请教GPS卡尔曼滤波传入测量量数值对处理结果的影响问题

   MatrixXd X=MatrixXd(4, 1);

   MatrixXd A=MatrixXd(4, 4);

   MatrixXd H=MatrixXd(2, 4);

   MatrixXd Q=MatrixXd(4, 4);

   MatrixXd R=MatrixXd(2, 2);

   MatrixXd P=MatrixXd(4, 4);

   MatrixXd K=MatrixXd(4, 2);

   MatrixXd Z=MatrixXd(2, 1);

   A.setZero();

   A(0,0) = 1;

   A(0,2) = 1;

   A(1,1) = 1;

   A(1,3) = 1;

   A(2,2) = 1;

   A(3,3) = 1;

   H.setZero();

   H(0.0) = 1;

   H(1,1) = 1;

   // H<< 1,0,0,0,

   //    0,1,0,0;

   Q.setZero();

   Q(0,0) = (double)0.5;      //系统噪声只存在位移上，速度方向没有---速度通过推测而来

   Q(1,1) = (double)0.5;

   // Q(2,2) = (double)0.2;

   // Q(3,3) = (double)0.2;

   // Q<<(double)0.2,           0,           0,             0,

   //    0,           (double)0.2,           0,             0,

   //    0,                      0,(double)0.2,             0,

   //    0,                      0,           0,  (double)0.2;  

   R.setZero();

   R(0,0) = (double)0.4; 

   R(1,1) = (double)0.4;

   // R<<(double)0.02,0,0,(double)0.02;

//    R.setZero();

   P.setZero();

   P(0,0) = 0.8;

   P(0,2) = 0.8;

   P(1,1) = 0.8;

   P(1,3) = 0.8;

   P(2,2) = 0.8;

   P(3,3) = 0.8;

   // P<<1,  0,  0,  0,

   //    0,  1,  0,  0,

   //    0,  0,  1,  0,

   //    0,  0,  0,  1; 

   K.setZero();  

   // K<< 0,0,0,0,0,0,0,0;

for(int i=0; i< MaxNum; i++)

   {



      X = kalfilter->kalman_input_exe(X,Z,0,Dt,R);

      // std::cout <<"OrgX: "<< x_buf[i] << " OrgY: " << y_buf[i] <<" O_Dx: "<< (x_buf[i] - x_buf[i-1])/Dt << " O_Dy: " << (y_buf[i] - y_buf[i-1])/Dt << " OtX: " << X(0,0) << " OtY: " << X(1,0) << " Dx: " << X(2,0) << " Dy: " << X(3,0) <<std::endl;

      // outfile << x_buf[i] << "  " << y_buf[i] <<"  "<< (x_buf[i] - x_buf[i-1])/Dt << "  " << (y_buf[i] - y_buf[i-1])/Dt << "  " << X(0,0) << "  " << X(1,0) << "  " << X(2,0)<< "  " << X(3,0)  <<std::endl;//输出到txt文档，用于matlab绘图

      std::cout <<"OrgX: "<< x_buf[i] << " OrgY: " << y_buf[i]  << " OtX: " << X(0,0) << " OtY: " << X(1,0) << " Dx: " << X(2,0) << " Dy: " << X(3,0) <<std::endl;

      outfile <<setw(10)<<setiosflags(ios::fixed)<<setprecision(2)<<x_buf[i] << "  " <<setw(10)<<setiosflags(ios::fixed)<<setprecision(2)<<y_buf[i] << "  " <<setw(10)<<setiosflags(ios::fixed)<<setprecision(2)<< X(0,0) << "  " <<setw(10)<<setiosflags(ios::fixed)<<setprecision(2)<< X(1,0) << "  " << X(2,0)<< "  " << X(3,0);     

      outfile << std::endl;//输出到txt文档，用于matlab绘图

      Z << x_buf[i],y_buf[i];

      R.setZero();

      R(0,0) =pow(lat_stddev[i],2);

      R(1,1) =pow(lon_stddev[i],2);

   }





MatrixXd kalman_input_exe(MatrixXd stata_X,MatrixXd z,double u, double time,MatrixXd R)

    {

        m_delta_time=time;

        m_R = R;

        // A<< 1,0,(double)delta_time,0,

        //     0,1,0,(double)delta_time,

        //     0,0,1,0,

        //     0,0,0,1;

        // B<<0,0,0,0;

        /* Predict */

        // X=A*X+B*u;

        m_X=m_A*stata_X;      //无控制量--预测 先验估算值

        //预测状态与真实状态的协方差矩阵，Pk'

        m_P=m_A*m_P*m_A.transpose() + m_Q;   //先验协方差

        // /*update*/

        //计算增益

        m_K = m_P*m_H.transpose()*(m_H*m_P*m_H.transpose()+m_R).inverse();

        m_X = m_X+m_K*(z-m_H*m_X);  //后验估算值

        m_P = m_P - m_K*m_H*m_P;    //后验协方差

        return m_X;

    }