滤波融合（一）基于C++完成一个简单的 卡尔曼滤波器 线性的系统和测量模型

在机器人定位中，KF或者EKF是常用的传感器融合算法，之前也总结过很多关于EKF的用法：
如何理解卡尔曼滤波(Kalman Filter)实现数据融合
通俗易懂理解扩展卡尔曼滤波EKF用于多源传感器融合

大部分都是基于MATLAB写的，所以正好有空简单的写一下基于C++的计算方法，并且和 bfl 进行对比。

简单的来说，EKF 分为两个过程，预测和更新，预测的部分一般使用的是数据频率比较高的传感器，例如IMU或者Odom数据，这里为了对比效果，使用了 bfl 例子中的仿真传感器数据，首先先对bfl中的例子，做一个简单的解析：

  for (time_step = 0; time_step < NUM_TIME_STEPS-1; time_step++)
    {
    
    
      auto start = std::chrono::steady_clock::now();
      // DO ONE STEP WITH MOBILE ROBOT
      mobile_robot.Move(input);
      // DO ONE MEASUREMEN
      ColumnVector measurement = mobile_robot.Measure();
      // UPDATE FILTER
      // filter.Update(&sys_model,input,&meas_model,measurement);
      filter.Update(&sys_model,input);
      filter.Update(&meas_model,measurement);
      measurement = mobile_robot.Measure();
      x = filter.PostGet()->ExpectedValueGet();  
      covarLabelMeas = filter.PostGet()->CovarianceGet();
      cout << " ExpectedValueGet = " <<  x <<" measurement = " <<  measurement << endl;

    } // estimation loop

其中 mobile_robot.Move(input) 就是其中仿真得到传感器数据，模拟了前进的距离。而 mobile_robot.Measure() 便是得到了该时刻的一个观测值。

那么便可以直接调用 EKF 中定义的模型，其中 filter.Update(&sys_model,input) 为预测值更新， 而 filter.Update(&meas_model,measurement) 为观测值更新，其中区分的地方就是模型的概率分布，在bfl库中会对其功能进行重载。

更新完之后就可以通过 filter.PostGet()->ExpectedValueGet 得到 EKF中的最优状态。

下面对模型中的协方差分布进行介绍，主要包括：先验，系统模型，观测值。简单来说，就是初始位置是否准确，在预测的过程中会引入多少误差，由于传感器硬件问题是否会带来观测数据的误差。

先验协方差：

  ColumnVector prior_Mu(2);
  prior_Mu(1) = PRIOR_MU_X;
  prior_Mu(2) = PRIOR_MU_Y;
  SymmetricMatrix prior_Cov(2);
  prior_Cov(1,1) = PRIOR_COV_X;
  prior_Cov(1,2) = 0.0;
  prior_Cov(2,1) = 0.0;
  prior_Cov(2,2) = PRIOR_COV_Y;
  Gaussian prior(prior_Mu,prior_Cov);

系统噪声：

  // create gaussian
  ColumnVector sysNoise_Mu(2);
  sysNoise_Mu(1) = MU_SYSTEM_NOISE_X;
  sysNoise_Mu(2) = MU_SYSTEM_NOISE_Y;

  SymmetricMatrix sysNoise_Cov(2);
  sysNoise_Cov = 0.0;
  sysNoise_Cov(1,1) = SIGMA_SYSTEM_NOISE_X;
  sysNoise_Cov(1,2) = 0.0;
  sysNoise_Cov(2,1) = 0.0;
  sysNoise_Cov(2,2) = SIGMA_SYSTEM_NOISE_Y;

  Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);

  // create the model
  LinearAnalyticConditionalGaussian sys_pdf(AB, system_Uncertainty);
  LinearAnalyticSystemModelGaussianUncertainty sys_model(&sys_pdf);

观测噪声：

  // Construct the measurement noise (a scalar in this case)
  ColumnVector measNoise_Mu(1);
  measNoise_Mu(1) = MU_MEAS_NOISE;

  SymmetricMatrix measNoise_Cov(1);
  measNoise_Cov(1,1) = SIGMA_MEAS_NOISE;
  Gaussian measurement_Uncertainty(measNoise_Mu, measNoise_Cov);

  // create the model
  LinearAnalyticConditionalGaussian meas_pdf(H, measurement_Uncertainty);
  LinearAnalyticMeasurementModelGaussianUncertainty meas_model(&meas_pdf);

最后就是 观测矩阵的构建，简单理解就是 所观测得到的数据，和 卡尔曼滤波 所需要的数据 之间的转换关系。

  // create matrix H for linear measurement model
  Matrix H(1,2);
  double wall_ct = 2/(sqrt(pow(RICO_WALL,2.0) + 1));
  H = 0.0;
  H(1,1) = wall_ct * RICO_WALL;
  H(1,2) = 0 - wall_ct;

例如观测值是距离墙面的距离，但是 KF 中所维护的状态量是 坐标值 (x,y) ，因此需要将距离转换成坐标值，但是这里定义的 运动模型就是距离，所以这些转换相当于是在运动模型中做了。该例子的运动模型如下：

  // Create the matrices A and B for the linear system model
  Matrix A(2,2);
  A(1,1) = 1.0;
  A(1,2) = 0.0;
  A(2,1) = 0.0;
  A(2,2) = 1.0;
  Matrix B(2,2);
  B(1,1) = cos(0.8);  // 0.69670670934717 
  B(1,2) = 0.0;
  B(2,1) = sin(0.8);  //0.71735609089952
  B(2,2) = 0.0;

以上就是 bfl 库中所做的融合过程。那么下面就 根据公式 自己实现一个 KF 滤波器。

---

首先 运动方程还是保持和原来不变，用以下的方程进行预测更新：
$$x_k=A_k{x}_{k-1}+B_k{u}_k+w_k$$
除了要更新 状态之外，还要更新该状态的一个协方差，也就是目前状态的一个可信度。也叫状态转移方程，其中 $Q_k$ 就是一个系统的噪声。
$$P_k=A_k\ast P_{k-1}\ast A^T+Q_k$$
以上预测的部分就完成了，下面便是更新的过程，也可以认为是融合的过程，两个高斯分布融合，其中首先要算卡尔曼增益，就是更相信谁的问题：
$$K=\frac{P_k\ast H}{H\ast P_k\ast H^T+R_k}$$
其中 $P_k$ 就是目前 KF 中所维护的状态的可信度，而 $R_k$ 就是观测值的可信度，那么计算得到的 $K$ 相当于是一个权重。
有了权重就可以知道更相信谁，那么如果更相信观测值，那么 KF 中所维护的状态 就要往观测值那边多靠一靠。
$$x_k=x_{k-1}+K(measurement-H\ast x_k)$$
这里也能看出来 $H$ 观测矩阵的用法，使观测值和状态值保持一致。

既然观测值被更新了，那么对应的置信度也需要被更新：
$$P_k=(I-K\ast H)\ast P_{k-1}$$

那么更新的过程也完成了，接下来就是一个不断循环迭代的过程。最终计算得到的效果是一模一样的～

cout << "MAIN: Starting estimation" << endl;

ColumnVector X_k(2); // 状态量 （x，y）
X_k= prior_Mu; // 先验
Matrix P(2,2); // 先验协防差
P = prior_Cov;

Matrix Identity(2,2); // 用于数据格式转换
Identity(1,1) = 1;
Identity(1,2) = 0.0;
Identity(2,1) = 0.0;
Identity(2,2) = 1;

for (time_step = 0; time_step < NUM_TIME_STEPS-1; time_step++)
  {
    
    
    auto start = std::chrono::steady_clock::now();
    // DO ONE STEP WITH MOBILE ROBOT
    mobile_robot.Move(input);
    // DO ONE MEASUREMEN
    ColumnVector measurement = mobile_robot.Measure();

    cout << " \n "<< endl;
    // 预测
    // 1.更新状态量
    X_k = A*X_k + B*input + sysNoise_Mu;
    // 2.协防差传播
    P = A*(P*A.transpose()) + sysNoise_Cov*Identity;
    
    // 观测值更新 
    Matrix K,R;
    R = measNoise_Cov;
    // 1.计算增益矩阵
    K = P*H.transpose() *( H*P*H.transpose() + R).inverse();
    // 2.更新状态量
    ColumnVector X_k_1(2);
    X_k = X_k + K *(measurement - H*X_k);
    // 3.协防差传播
    P = (Identity - K*H)*P;

    cout << " K:" << K  <<endl;
    cout << " Z - Hx:" << measurement - H*X_k <<endl;
    cout << " K(Z - Hx):" << K *(measurement - H*X_k) <<endl;
    cout << " x + K(Z - Hx):" << X_k <<endl;

  } // estimation loop

具体的代码如上，后面会继续更新基于C++的非线性卡尔曼滤波，也就是考虑了转向角。