C++ 卡尔曼滤波

• 以行人为例，可测量的数据是二维坐标

#include 
#include 
#include 

using namespace Eigen;
using namespace std;

class Kalman
{
     
public:
    Kalman() : is_init_(false) {
     }
    ~Kalman() {
     }
    void Init(float mx, float my) {
     
        VectorXf tmp(4,1);
        tmp << mx, my, 0, 0;
        x_ = tmp;
        // F_需要更新时间
        F_ = MatrixXf::Identity(4, 4);
        // 位置和速度的协方差矩阵，会随迭代变准确，初始值可以随意一点
        P_ = 100 * MatrixXf::Identity(4, 4);
        // 预测误差协方差矩阵，行走的话主要是加速度
        MatrixXf q(4,4);
        q << 6.25e-6, 6.25e-6, 1.25e-4, 1.25e-4, 
             6.25e-6, 6.25e-6, 1.25e-4, 1.250e-4, 
             1.25e-4, 1.25e-4, 2.5e-3,  2.5e-3, 
             1.25e-4, 1.25e-4, 2.5e-3,  2.5e-3;
        Q_ = MatrixXf::Identity(4, 4);
        // 状态变量到测量的转换矩阵，即位置和速度转换成位置
        MatrixXf h(2, 4);
        h << 1.0, 0, 0, 0,
             0.0, 1, 0, 0;
        H_ = h;
        // 测量噪声协方差矩阵, 根据传感器算的
        MatrixXf r(2,2);
        r << 0.0225, 0, 
             0, 0.0225;
        R_ = r;
    }
    void Predict()
    {
     
        // 预测
        x_ = F_ * x_;
        MatrixXf f = F_.transpose();
        // 更新状态协方差矩阵
        P_ = F_ * P_ * f + Q_;
    }
    void MeasureUpdate(const VectorXf &z)
    {
     
        VectorXf y = z - H_ * x_;
        // 计算卡尔曼增益
        MatrixXf S = H_ * P_ * H_.transpose() + R_;
        MatrixXf K = P_ * H_.transpose() * S.inverse();
        // 最优估算
        x_ = x_ + (K * y);
        int size = x_.size();
        MatrixXf I = MatrixXf::Identity(size, size);
        // 
        P_ = (I - K * H_) * P_;
    }

    bool is_init_;
    VectorXf x_;
    MatrixXf F_;
    MatrixXf P_;
    MatrixXf Q_;
    MatrixXf H_;
    MatrixXf R_;
};

int main()
{
     
    float x[10] = {
     1.0, 1.1, 1.2, 1.3, 1.4, 1.6, 1.8, 2.2, 2.3, 2.7};
    float y[10] = {
     1.0, 1.1, 1.2, 1.3, 1.5, 1.6, 1.8, 2.0, 2.3, 2.7};
    float t[10] = {
     0.1, 0.2, 0.3, 0.4, 0.5, 0.7, 0.9, 1.1, 1.3, 1.9};
    Kalman k;
    ifstream data("/home/coor.txt");
    float mx, my;
    float last_time;
    for(int i = 0; i < 55; ++i) {
     
        if(!k.is_init_) {
     
            k.is_init_ = true;
            // last_time = t[i];
            data >> mx >> my;
            k.Init(mx, my);
            // VectorXf tmp(4,1);
            // tmp << mx, my, 0, 0;
            // k.x_ = tmp;
            // MatrixXf p(4, 4);
            // p << 1.0, 0, 0, 0,
            //         0.0, 1, 0, 0,
            //         0.0, 0, 100, 0,
            //         0.0, 0, 0, 100;
            // k.P_ = p;
            // MatrixXf q(4,4);
            // q << 1.0, 0, 0, 0,
            //         0.0, 1, 0, 0,
            //         0.0, 0, 1, 0,
            //         0.0, 0, 0, 1;
            // k.Q_ = q;
            // MatrixXf h(2, 4);
            // h << 1.0, 0, 0, 0,
            //         0.0, 1, 0, 0;
            // k.H_ = h;
            // MatrixXf r(2,2);
            // r << 0.0225, 0, 0, 0.0225;
            // k.R_ = r;
            continue;
        }
        // float delta = t[i] - last_time;
        // last_time = t[i];
        MatrixXf f(4,4);
        f << 1.0, 0, 0.1, 0,
               0.0, 1, 0, 0.1,
               0.0, 0, 1, 0,
               0.0, 0, 0, 1;
        k.F_ = f;
        k.Predict();
        cout << i + 1 << "\t" << k.x_(0,0) << "\t" << k.x_(1,0) << endl;
        // 
        VectorXf z(2, 1);
        data >> mx >> my;
        z << mx, my;
        cout << i + 1 << "\t" << k.x_(0,0) - mx << "\t" << k.x_(1,0) - my << endl;
        k.MeasureUpdate(z);
        // cout << i + 1 << "\t";
        // std::cout << k.x_ << std::endl;
    }
    return 0;
}

• 三个协方差矩阵的理解可看这里
• 调参看这里
• 提供一组行人数据：时间–x--y

3.2 -31.8 -2.01
3.3 -31.7 -1.98
3.4 -31.6 -1.96
3.55 -31.6 -1.94
3.7 -31.6 -1.91
3.84 -31.5 -1.88
3.92 -31.4 -1.85
4.02 -31.4 -1.81
4.11 -31.3 -1.79
4.43 -31.3 -1.66
4.53 -31.3 -1.6
4.61 -31.2 -1.52
4.7 -31.2 -1.44
4.8 -31.2 -1.34
5.21 -31.1 -1.13
5.3 -31.1 -1.08
5.4 -31.1 -1.01
5.51 -31.1 -0.921
5.62 -31.1 -0.833
5.7 -31.1 -0.754
5.81 -31.1 -0.683
6.02 -31.1 -0.604
6.24 -31.2 -0.479
6.33 -31.2 -0.382
6.9 -31.2 -0.126
7.23 -31.3 0.0793
7.6 -31.4 0.204
7.8 -31.5 0.329
8.23 -31.5 0.576
8.35 -31.6 0.647
8.6 -31.6 0.772
8.8 -31.6 0.922
9.31 -31.7 1.07
9.52 -31.7 1.2
10.09 -31.8 1.49
10.31 -31.8 1.62
10.44 -31.9 1.75
11 -31.9 1.99
11.22 -32.2 2.12
11.42 -31.9 2.25
11.82 -32 2.35
11.99 -32 2.48
12.31 -31.9 2.62
12.48 -31.9 2.58
12.82 -31.9 2.68
13.01 -31.9 2.79
13.29 -31.9 2.91
13.71 -31.8 3.03
13.92 -31.8 3.16
14.2 -31.8 3.23
14.5 -31.8 3.34
14.79 -31.7 3.46
14.99 -31.6 3.47
15.3 -31.5 3.47
15.47 -31.3 3.51
15.8 -31.2 3.58
16.09 -31.1 3.67
16.4 -30.9 3.82
16.67 -30.8 3.83
16.92 -30.7 3.83
17.19 -30.5 3.89
17.49 -30.4 3.9
17.81 -30.2 3.93
18.11 -30.1 3.89
18.49 -29.9 3.89
18.71 -29.8 3.83
18.91 -29.6 3.82
19.42 -29.5 3.8
19.6 -29.4 3.83
19.82 -29.3 3.83
20.2 -29.1 3.84
20.42 -29.1 3.84
20.63 -28.9 3.79
21.87 -28.8 3.72