基于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 x_k​=A_k​x_{k−1}+B_k​u_k​+w_k xk​​=Ak​​xk−1​+Bk​​uk​​+wk​
除了要更新 状态之外，还要更新该状态的一个协方差，也就是目前状态的一个可信度。也叫状态转移方程，其中 Q k Q_k Qk​ 就是一个系统的噪声。
P k = A k ∗ P k − 1 ∗ A T + Q k P_{k} = A_k*P_{k-1}*A^T + Q_k Pk​=Ak​∗Pk−1​∗AT+Qk​
以上预测的部分就完成了，下面便是更新的过程，也可以认为是融合的过程，两个高斯分布融合，其中首先要算卡尔曼增益，就是更相信谁的问题：
K = P k ∗ H H ∗ P k ∗ H T + R k K = {P_k*H over H*P_k*H^T + R_k} K=H∗Pk​∗HT+Rk​Pk​∗H​
其中 P k P_k Pk​ 就是目前 KF 中所维护的状态的可信度，而 R k R_k Rk​ 就是观测值的可信度，那么计算得到的 K K K 相当于是一个权重。
有了权重就可以知道更相信谁，那么如果更相信观测值，那么 KF 中所维护的状态 就要往观测值那边多靠一靠。
x k = x k − 1 + K ( m e a s u r e m e n t − H ∗ x k ) x_k = x_{k-1} +K(measurement -H*x_k) xk​=xk−1​+K(measurement−H∗xk​)
这里也能看出来 H H H 观测矩阵的用法，使观测值和状态值保持一致。

既然观测值被更新了，那么对应的置信度也需要被更新：
P k = ( I − K ∗ H ) ∗ P k − 1 P_k = (I - K*H)*P_{k-1} Pk​=(I−K∗H)∗Pk−1​

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

      X_k = A*X_k + B*input;
      P = A*(P*A.transpose()) + sysNoise_Cov*Identity;

      Matrix K,R;
      R = measNoise_Cov;
      K = P*H.transpose() *( H*P*H.transpose() + R).inverse();
      
      ColumnVector X_k_1(2);
      X_k = X_k + K *(measurement - H*X_k);
      P = (Identity - K*H)*P;

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