C++实现对GPS读取坐标的卡尔曼滤波

通过网上很多资料,还有例程的代码,我根据自己的需求写的笔记如下
用自己的话来讲,卡尔曼滤波就是找一个权重值,我有一个测量值和一个预测值,测量值就是我通过各种传感器得到的数据,例如速度呀,加速度呀,经纬度这些,这个数据会收到外界干扰,和理论值存在一些偏差,还有一个是预测值,预测值是怎么得到的呢?第一个预测值肯定是自己想的预测的是多少,后面的预测值就要根据不断变化的系数进行修正,这个让他变化的东西就是状态转移方程,一次次的更新我们的预测值.综合以上考虑,我们需要得到一个最终结果,这个值就叫做估计值,估计值等于一定比例的测量值＋预测值.我更相信测量值,那我就让测量值占0.7,预测值占0.3,如果一半一半,那就测量值占0.5,预测值也占0.5,我们需要找的就是一个这样最合适的比例.公式还是有很多数学基础知识,建议推导过程把所有不会的都查一遍,这样才能更好的理解五个卡尔曼公式,最终编写代码

我自己编写的代码如下,不过只考虑的X方向上的滤波,Y方向的没有考虑,如法炮制即可

void RasterWindow:: Kalman(std::vector<std::vector<double>> &gpspoints)
{
    const double Time = 1; //控制周期1s
    MatrixXd A(2,2); // 系数矩阵
    A(0, 0) = 1;
    A(1, 0) = 0;
    A(0, 1) = Time;
    A(1, 1) = 1;

    MatrixXd H(1,2);   //观测矩阵
    H(0, 0) = 1;
    H(0, 1) = 0;
    MatrixXd Q(2, 2);//过程激励噪声协方差，假设系统的噪声向量只存在速度分量上，且速度噪声的方差是一个常量0.01，位移分量上的系统噪声为0
    Q(0, 0) = 0;
    Q(1, 0) = 0;
    Q(0, 1) = 0;
    Q(1, 1) = 0.01;
    MatrixXd R(1, 1);//观测噪声协方差，测量值只有位移，它的协方差矩阵大小是1*1，就是测量噪声的方差本身。
        R(0, 0) = 0.00001;
        //变量定义阵，初始值均为零
    MatrixXd X_evlt = MatrixXd::Constant(2, 1, 0); //状态预测值
    MatrixXd X_pdct = MatrixXd::Constant(2, 1, 0); //状态估计值
    MatrixXd Z_meas = MatrixXd::Constant(1, 1, 0); //测量值
    MatrixXd X_real(2, 1);  //真实值

    MatrixXd Pk = MatrixXd::Constant(2, 2, 0);  //预测状态与真实状态的协方差矩阵
    MatrixXd Pk_p = MatrixXd::Constant(2, 2, 0);//估计状态和真实状态的协方差矩阵
    MatrixXd K = MatrixXd::Constant(2, 1, 0);  //kalman增益矩阵
    MatrixXd tmp(1, 1);
    std::vector<MatrixXd> x_evlt, x_pdct, z_meas, pk, pk_p, k,x_real;
    for (int i = 1; i < GPSNUM; ++i) {
            X_real(0,0)=gpspoints[i-1][0]+gpspoints[i-1][2]*Time*0.00001;
            X_real(1,0)=0;
            x_real.push_back(X_real);
        }
    x_evlt.push_back(X_evlt);
    x_pdct.push_back(X_pdct);
    z_meas.push_back(Z_meas);
    pk.push_back(Pk);
    pk_p.push_back(Pk_p);
    k.push_back(K);

    //开始迭代
       for (int i = 1; i < GPSNUM-1; i++) {
            //预测值
            X_pdct = A * x_evlt[i - 1];
            x_pdct.push_back(X_pdct);
            //预测状态与真实状态的协方差矩阵，Pk'
            Pk_p = A * pk[i - 1] * A.transpose() + Q;
            pk_p.push_back(Pk_p);
            //K:2x1
            tmp = H * pk_p[i] * H.transpose() + R;
            K = pk_p[i] * H.transpose() * tmp.inverse();
            k.push_back(K);
            //测量值z
            Z_meas = H * x_real[i];
            z_meas.push_back(Z_meas);
            //估计值
            X_evlt = x_pdct[i] + k[i] * (z_meas[i] - H * x_pdct[i]);
            x_evlt.push_back(X_evlt);
            //估计状态和真实状态的协方差矩阵，Pk
            Pk = (MatrixXd::Identity(2, 2) - k[i] * H) * pk_p[i];
            pk.push_back(Pk);
        }
        std::cout << "含噪声测量" << "  " << "后验估计" << "  " << "真值" << "  " << std::endl;
        for (int i = 0; i < GPSNUM-1; i++) {
            std::cout<<z_meas[i]<<"  "<<x_evlt[i](0,0)<<"  "<<x_real[i](0,0)<<std::endl;
            gpspoints[i][0] = x_evlt[i](0,0);

}


}