Ejemplo n.º 1
0
        /// <summary>
        /// Provides the unscented transform estimate for the array X = [x_0,...,x_N] of random variable x samples
        /// given the array of observations Y = [y_0,...,y_N], where y_i = Phi(x_i) + Nu_i.
        /// Parameters of the unscented transform utParams should be initialized.
        /// </summary>
        /// <param name="Phi">Transformation: a nonlinear function which determines the transformation of the random vector variable: y = Phi(x) + nu</param>
        /// <param name="X">Array of initial variable x samples</param>
        /// <param name="Y">Array of transformed variable y = Phi(x) + nu samples</param>
        /// <param name="mX">Mean of x</param>
        /// <param name="KX">Cov of x</param>
        /// <param name="KNu">Cov of the noize nu</param>
        /// <param name="mErr_UT">Returns: estimation error mean vector</param>
        /// <param name="KErr_UT">Returns: estimation error covariance marix</param>
        /// <param name="KErrTh_UT">Returns: estimation error theoretical covariance marix</param>
        /// <returns>Array of estimates \hat{X} = [\hat{x}_0,...,\hat{x}_N]</returns>
        public Vector <double>[] Estimate(Func <Vector <double>, Vector <double> > Phi, Vector <double>[] X, Vector <double>[] Y, Vector <double> mX, Matrix <double> KX, Matrix <double> KNu,
                                          out Vector <double> mErr_UT, out Matrix <double> KErr_UT, out Matrix <double> KErrTh_UT)
        {
            UnscentedTransform.Transform(x => Phi(x), mX, KX, KNu, utParams, out Vector <double> M_UT, out Matrix <double> Kxy_UT, out Matrix <double> Kyy_UT);
            Matrix <double> P_UT = Kxy_UT * ((Kyy_UT).PseudoInverse());

            KErrTh_UT = KX - P_UT * Kyy_UT * P_UT.Transpose();

            Vector <double>[] Xhat_UT = Y.Select(y => mX + P_UT * (y - M_UT)).ToArray();
            Vector <double>[] Err_UT  = Xhat_UT.Subtract(X);
            mErr_UT = Err_UT.Average();
            KErr_UT = Exts.Cov(Err_UT, Err_UT);

            return(Xhat_UT);
        }
Ejemplo n.º 2
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());
        }
Ejemplo n.º 3
0
        /// <summary>
        /// Calculates the criterion value for the estimate given the particular unscented transform parameters
        /// </summary>
        /// <param name="Phi">Transformation: a nonlinear function which determines the transformation of the random vector variable: y = Phi(x) + nu</param>
        /// <param name="Crit">Criterion: a function which determines the quality of the unscented transform estimate. Depends on the sample covariance of the estimation error: val = Crit(Cov(X-Xhat,X-Xhat))  </param>
        /// <param name="p">Unscented transform parameters</param>
        /// <param name="X">Array of initial variable x samples</param>
        /// <param name="Y">Array of transformed variable y = Phi(x) + nu samples</param>
        /// <param name="mX">Mean of x</param>
        /// <param name="KX">Cov of x</param>
        /// <param name="KNu">Cov of the noize nu</param>
        /// <returns>The criterion value for the particular unscented transform parameters</returns>
        public static double CalculateCriterionValue(Func <Vector <double>, Vector <double> > Phi, Func <Matrix <double>, double> Crit, UTParams p, Vector <double>[] X, Vector <double>[] Y, Vector <double> mX, Matrix <double> KX, Matrix <double> KNu)
        {
            double crit = 0;

            try
            {
                UnscentedTransform.Transform(x => Phi(x), mX, KX, KNu, p, out Vector <double> M_UT, out Matrix <double> Kxy_UT, out Matrix <double> Kyy_UT);
                Matrix <double> P_UT      = Kxy_UT * ((Kyy_UT).PseudoInverse());
                Matrix <double> KErrTh_UT = KX - P_UT * Kyy_UT * P_UT.Transpose();

                Vector <double>[] Xhat_UT = Y.Select(y => mX + P_UT * (y - M_UT)).ToArray();
                Vector <double>[] Err_UT  = Xhat_UT.Subtract(X);
                Vector <double>   mErr_UT = Err_UT.Average();
                Matrix <double>   KErr_UT = Exts.Cov(Err_UT, Err_UT);
                crit = Crit(KErr_UT);//.Trace();
            }
            catch { crit = double.MaxValue; }
            return(crit);
        }
Ejemplo n.º 4
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);
        }