C++实现卡尔曼滤波

/** 
 * Write a function 'filter()' that implements a multi-
 *   dimensional Kalman Filter for the example given
 */

#include <iostream>
#include <vector>
// Eigen 部分
#include <Eigen/Core>
#include <Eigen/Dense>

using std::cout;
using std::endl;
using std::vector;
using Eigen::VectorXd;
using Eigen::MatrixXd;

// Kalman Filter variables
VectorXd x;	// object state 障碍物状态矩阵
MatrixXd P;	// object covariance matrix 障碍物不确定性协方差
VectorXd u;	// external motion 外部的运动
MatrixXd F; // state transition matrix 状态转移矩阵
MatrixXd H;	// measurement matrix 测量矩阵
MatrixXd R;	// measurement covariance matrix 测量协方差矩阵
MatrixXd I; // Identity matrix 单位矩阵
MatrixXd Q;	// process covariance matrix 过程协方差矩阵

vector<VectorXd> measurements; // 测量值
void filter(VectorXd &x, MatrixXd &P); // 滤波函数


int main() {
  /**
   * Code used as example to work with Eigen matrices
   */
  // design the KF with 1D motion
  x = VectorXd(2); // 初始化障碍物状态矩阵 第一个为位置，第二个为速度
  x << 0, 0;

  P = MatrixXd(2, 2); // 初始化不确定性协方差矩阵， 刚开始，位置(0,0)的不确定性为1000，速度的不确定性为1000
  P << 1000, 0, 0, 1000;

  u = VectorXd(2); // 运动误差
  u << 0, 0;

  F = MatrixXd(2, 2); // 状态转移矩阵初始化
  F << 1, 1, 0, 1;

  H = MatrixXd(1, 2); // 测量矩阵
  H << 1, 0;

  R = MatrixXd(1, 1); // 测量协方差矩阵
  R << 1;

  I = MatrixXd::Identity(2, 2); // 单位矩阵

  Q = MatrixXd(2, 2); // 过程协方差矩阵
  Q << 0, 0, 0, 0;

  // create a list of measurements
  VectorXd single_meas(1); 
  single_meas << 1;
  measurements.push_back(single_meas);
  single_meas << 2;
  measurements.push_back(single_meas);
  single_meas << 3;
  measurements.push_back(single_meas);

  // call Kalman filter algorithm
  filter(x, P);

  return 0;
}


void filter(VectorXd &x, MatrixXd &P) {

  for (unsigned int n = 0; n < measurements.size(); ++n) {

    VectorXd z = measurements[n];
    // TODO: YOUR CODE HERE
    /**
     * KF Measurement update step
     */
    VectorXd y = z - H * x;
    MatrixXd Ht = H.transpose();
    MatrixXd S = H * P * Ht + R;
    MatrixXd Si = S.inverse();
    MatrixXd K = P * Ht * Si;

    // new state
    x = x + (K * y);
    P = (I - K * H) * P;

    /**
     * KF Prediction step
     */
    x = F * x + u;
    MatrixXd Ft = F.transpose();
    P = F * P * Ft + Q;

    cout << "x=" << endl
         << x << endl;
    cout << "P=" << endl
         << P << endl;
  }
}

首先，让我们快速复习一下卡尔曼滤波一维运动的例子。比如我们的目标是跟踪一个行人，他的状态时$x$，$x$由位置和速度表示，$$x=\left(\begin{array}{c}p\\ v\end{array}\right)$$

预测步骤

当我们设计卡尔曼滤波，我们必须定义两个线性方程: 状态转移方程和测量方程。状态转移方程就是:
$$x^{\prime }=F\ast x+noise$$
F就是
$$F=\left(\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right)$$
$x^{\prime }$就是预测目标在$\Delta t$之后的状态。
$F$是状态转移矩阵，与$x$相乘获得$\Delta t$之后的目标状态。
线性运动模型通过恒定速度速度,新的位置来计算$p^{\prime }$，$p^{\prime }$计算为:
$$p^{\prime }=p+v\ast \Delta t$$
这里的$p$是旧位置，$v$是速度，因为速度恒定，所以新速度和旧速度一样$v^{\prime }=v$。
我们用矩阵的形式表达如下：
$$\left(\begin{array}{c}{p}^{\prime }\\ v^{\prime }\end{array}\right)=\left(\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right)\left(\begin{array}{c}p\\ v\end{array}\right)$$
我们将目标的位置和速度是服从均值为$x$的高斯分布。所以它可以代入卡尔曼等式$x^{\prime }=Fx+noise$，我们计算状态向量的均值。噪声(noise)依然符合高斯分布，不过均值是0，因此noise=0也就是说噪声均值是0，所以等式就变成了$x^{\prime }=Fx$。

因为噪声是不确定的，不确定性在Q矩阵中是作为加速度噪声。

更新步骤

作为更新步骤，我们使用测量函数来将状态向量映射到传感器的测量空间中。也就是将高维的状态信息变成低维的传感器的空间维度。举个例子，比如激光雷达只测量目标位置，但是扩张卡尔曼滤波建模目标的位置和速度。所以需要乘以$H$矩阵从状态向量$x$中将速度信息去掉。那么雷达测量的目标位置和我们预测的目标位置就可以进行比较了。
$$z=H\ast x+\omega$$
其中$\omega$代表的是传感器测量的传感器噪声。
所以对于激光雷达来说，测量方程就如下所示：
$$z=p^{\prime }$$
用矩阵来表示如下:
$$z=\left(\begin{array}{cc}1& 0\end{array}\right)\left(\begin{array}{c}{p}^{\prime }\\ v^{\prime }\end{array}\right)$$
我们都知道，一般算法都是由包含预测新状态和协方差$P^{\prime }$组成的预测步骤组成。我们还有包含用最新的测量来更新我们的估计和不确定性的测量更新步骤组成。