Пример #1
0
        public static (Vector <double>, Matrix <double>) Step(Func <int, Vector <double>, Vector <double> > Phi,
                                                              Func <int, Vector <double>, Vector <double> > Psi,
                                                              Matrix <double> Rw,
                                                              Matrix <double> Rnu,
                                                              UTParams p1,
                                                              UTParams p2,
                                                              int t,
                                                              Vector <double> y,
                                                              Vector <double> xHat_,
                                                              Matrix <double> P_)
        {
            UnscentedTransform.Transform(x => Phi(t, x), xHat_, P_, Rw, p1, out Vector <double> Xtilde, out _, out Matrix <double> Ptilde);
            UnscentedTransform.Transform(x => Psi(t, x), Xtilde, Ptilde, Rnu, p2, out Vector <double> Ytilde, out Matrix <double> PXY, out Matrix <double> PYtilde);
            Matrix <double> K = PXY * PYtilde.Inverse();

            return(Xtilde + K * (y - Ytilde), Ptilde - K * PYtilde * K.Transpose());
        }
Пример #2
0
        /// <summary>
        /// Performs a step of Unscented Kalman Filter given the particular unscented transform parameters
        /// for forecast and correction phases
        /// </summary>
        /// <param name="Phi1">State transformation: a nonlinear function which determines the dynamics: x_{t+1} = Phi_1(x_t) + Phi_2(x_t) W_t</param>
        /// <param name="Phi2">Noise multiplicator in the dynamics equation: x_{t+1} = Phi(x_t) + W_t</param>
        /// <param name="Psi1">Observations transformation: a nonlinear function which determines the relation between the state and the observations: y_t = Psi_1(x_t) + Psi_2(x_t) Nu_t</param>
        /// <param name="Psi2">Noise multiplicator in the observations equation: y_t = Psi_1(x_t) + Psi_2(x_t) Nu_t</param>
        /// <param name="Mw">Mean of the noise in the dynamics equation </param>
        /// <param name="Rw">Covariance matrix of the state disturbances</param>
        /// <param name="Mnu">Mean of the noise in the obseration equation </param>
        /// <param name="Rnu">Convariance matrix of the observation noise</param>
        /// <param name="p1">Unscented transfrom parameters for the forecast phase</param>
        /// <param name="p2">Unscented transfrom parameters for the correction phase</param>
        /// <param name="t">Current step time instant</param>
        /// <param name="y">Observations on the current step</param>
        /// <param name="xHat_">Estimate on the previous step</param>
        /// <param name="P_">Approximated previous step error covariance</param>
        /// <param name="xHat">Returns: current step estimate</param>
        /// <param name="P">Returns: approximated current step error covariance</param>
        public static (Vector <double>, Matrix <double>) Step(Func <int, Vector <double>, Vector <double> > Phi1,
                                                              Func <int, Vector <double>, Matrix <double> > Phi2,
                                                              Func <int, Vector <double>, Vector <double> > Psi1,
                                                              Func <int, Vector <double>, Matrix <double> > Psi2,
                                                              Vector <double> Mw,
                                                              Matrix <double> Rw,
                                                              Vector <double> Mnu,
                                                              Matrix <double> Rnu,
                                                              UTParams p1,
                                                              UTParams p2,
                                                              int t,
                                                              Vector <double> y,
                                                              Vector <double> xHat_,
                                                              Matrix <double> P_)
        {
            try
            {
                UnscentedTransform.Transform(x => Phi1(t, x) + Phi2(t, x) * Mw, xHat_, P_, Phi2(t, xHat_) * Rw * Phi2(t, xHat_).Transpose(), p1, out Vector <double> Xtilde, out _, out Matrix <double> Ptilde);
                //UnscentedTransform.Transform(x => Phi1(t, x), xHat_, P_, Rw, p1, out Vector<double> Xtilde2, out _, out Matrix<double> Ptilde2);
                UnscentedTransform.Transform(x => Psi1(t, x) + Psi2(t, x) * Mnu, Xtilde, Ptilde, Psi2(t, Xtilde) * Rnu * Psi2(t, Xtilde).Transpose(), p2, out Vector <double> Ytilde, out Matrix <double> PXY, out Matrix <double> PYtilde);
                //UnscentedTransform.Transform(x => Psi1(t, x), Xtilde, Ptilde, Rnu, p2, out Vector<double> Ytilde2, out Matrix<double> PXY2, out Matrix<double> PYtilde2);

                Matrix <double> K = PXY * PYtilde.Inverse();
                return(Xtilde + K * (y - Ytilde), Ptilde - K * PYtilde * K.Transpose());
            }
            catch (Exception e)
            {
                Console.WriteLine(e.Message);
                return(xHat_, P_);
            }
            //Matrix<double> K2 = PXY2 * PYtilde2.Inverse();
            //Vector<double> xHat2 = Xtilde2 + K2 * (y - Ytilde2);
            //Matrix<double> PHat2 = Ptilde2 - K2 * PYtilde2 * K2.Transpose();

            //Console.WriteLine(xHat - xHat2);
            //Console.WriteLine(PHat - PHat2);
        }