Пример #1
0
        /// <summary>
        /// 在 GNSS 动态导航中的应用
        /// </summary>
        protected void Motion()
        {
            List <string> paramNames = new List <string>()
            {
                "X", "Y", "Z", "dX", "dY", "dZ"
            };
            ///参数
            List <Vector> satXyzs  = new List <Vector>();
            Vector        siteXyz  = new Vector(3);
            Vector        L        = new Vector(6); //观测值
            double        interval = 30;            //时间间隔(单位:秒)


            //将瞬时加速度作为随机干扰,
            //状态向量, 位置和速度 X = trans([x, y, z, vx, vy, vz])
            //噪声向量, 为加速度 Noise = trans(ax, ay, az])
            //纯量形式, y = y0 + t * v0 + 0.5 * a0 * a0 * t
            //           v =          v0 +            a0 * t
            /// 状态方程
            ///         ┌I Δt┐          ┌0.5*Δt*Δt┐
            ///  X(k) = │     │X(k-1) +  │           │Ω(k-1)
            ///         └0  I ┘          └    Δt    ┘
            ///  观测方程
            ///  L(k) = A(k) X(k) + Ω(k)
            ///  其中,L(k)为 k 时刻系统的 C/A 码观测向量

            //常用量
            double[][] identity3x3 = MatrixUtil.CreateIdentity(3);
            int        satCount    = satXyzs.Count;

            //状态方程状态转移矩阵 Φ(k,k-1)
            //  double[][] trans = MatrixUtil.CreateIdentity(6);
            ArrayMatrix trans = new ArrayMatrix(6, 6);

            MatrixUtil.SetSubMatrix(trans.Array, MatrixUtil.GetMultiply(identity3x3, interval), 0, 3);
            //状态方程噪声向量Ω(k-1)的系数阵
            double[][] coeef_noise = MatrixUtil.Create(6, 3);
            MatrixUtil.SetSubMatrix(coeef_noise, MatrixUtil.GetMultiply(identity3x3, 0.5 * interval * interval));
            MatrixUtil.SetSubMatrix(coeef_noise, MatrixUtil.GetMultiply(identity3x3, interval), 3);

            //观测方程系数矩阵 A,每个时刻不同
            ArrayMatrix coeef_obs = new ArrayMatrix(satCount, 6);

            for (int i = 0; i < satCount; i++)
            {
                IVector differXyz = (satXyzs[i] - siteXyz);
                coeef_obs[i, 0] = differXyz.GetCos(0);//.CosX;
                coeef_obs[i, 1] = differXyz.GetCos(1);
                coeef_obs[i, 2] = differXyz.GetCos(2);
            }
            KalmanFilter lastKf = null;

            //Vector lastParams = lastKf.Corrected ?? null;
            //IMatrix lastParamCova = lastKf.Corrected.InverseWeight ?? null;

            //KalmanFilter kf = new KalmanFilter(coeef_obs, L, trans, lastParams, lastParamCova);
            //kf.Process();
        }
Пример #2
0
 /// <summary>
 /// 构造函数
 /// </summary>
 public RecursiveAdjuster()
 {
     NormalEquationSuperposer = new NormalEquationSuperposer();
     MatrixAdjuster           = new KalmanFilter();
     this.StepOfRecursive     = StepOfRecursive.SuperposOfConstNeq;
 }