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 motionx = VectorXd(2); // 初始化障碍物状态矩阵 第一个为位置，第二个为速度x << 0, 0;P = MatrixXd(2, 2); // 初始化不确定性协方差矩阵， 刚开始，位置(0,0)的不确定性为1000，速度的不确定性为1000P << 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 measurementsVectorXd 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 algorithmfilter(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 statex = 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;}
}

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

预测步骤

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

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

更新步骤

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