C++ 卡尔曼滤波

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

#include <iostream>
#include <eigen3/Eigen/Dense>
#include <fstream>

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