基于MATLAB&SIMULINK开发自动驾驶系统第四讲之线性卡尔曼滤波
卡尔曼滤波是目前行业中做感知算法和感知融合算法必用的算法，因此小明师兄和大家一起来学习一下。
当使用卡尔曼滤波器跟踪物体时，会使用一连串的探测量或测量量来构建物体运动的模型。物体运动是由物体状态的变化来定义的。卡尔曼滤波器是一种最佳的、递归的算法，用于估计物体的运动跟踪。滤波器之所以是递归的，是因为它使用之前的状态，使用可能在该区间内进行的测量来更新当前状态。卡尔曼滤波器将这些新的测量值纳入其中，以保持状态估计的尽可能准确。该滤波器是最佳的，因为它使状态的均方误差最小化。可以使用该滤波器来预测未来的状态或估计当前状态或过去的状态。
4.1 状态方程
对于自动驾驶工具箱中跟踪的大多数类型的物体，状态向量由一维、二维或三维的位置和速度组成。
从物体在x方向以恒定加速度运动的牛顿方程开始，将这些方程转换为空间状态形式：
如果定义状态作为：
可以将牛顿定律写成如下状态方程：
当对物体遵循这种类型的运动有信心时，会使用线性动态模型。有时，模型包括过程噪声，以反映运动模型的不确定性。在这种情况下，牛顿方程有一个附加项：
vk是加速度的未知噪声扰动。只有噪声的统计量是已知的。假设它是零均值的高斯白噪声。
可以将这种类型的方程扩展到一个以上的维度。在二维中，方程的形式为：
右侧的4×4矩阵是状态转换模型矩阵。对于独立的x和y运动，这个矩阵是块对角线。
当过渡到离散时间时，会在时间间隔的长度上整合运动方程。在离散形式中，对于T的样本区间，状态表示变成了：
量xk+1是离散时间k+1时的状态，xk是更早的离散时间k时的状态。如果加入噪声，方程就会变得更复杂，因为噪声的积分并不简单。
状态方程可以概括为：
Fk是状态转换矩阵，Gk是控制矩阵。控制矩阵考虑了作用在物体上的任何已知力。这两个矩阵均已给出。最后一个项代表动态模型的类似噪声的随机扰动。噪声被假定为零均值高斯白噪声。
有输入噪声的连续时间系统由线性随机微分方程描述。有输入噪声的离散时间系统用线性随机微分方程描述。状态空间表示法是一个物理系统的数学模型，其中输入、输出和状态变量由一阶耦合方程相关。
4.2 测量模型
测量是对系统的观察。测量值取决于状态矢量，但并不总是与状态矢量相同。例如，在雷达系统中，测量值可以是球面坐标，如距离、方位角和海拔，而状态矢量是笛卡尔坐标的位置和速度。对于线性卡尔曼滤波器，测量值总是状态矢量的线性函数，排除了球面坐标。要使用球面坐标，需要使用扩展卡尔曼滤波器。
测量模型假设任何时候的实际测量值都与当前状态有关，其方法是：
wk代表当前时间步的测量噪声。测量噪声也是零均值白高斯噪声，其协方差矩阵Q由以下方式描述：
4.3 线性卡尔曼滤波等式
如果没有噪声，则卡尔曼滤波的等式应该为：
同样，测量模型也没有测量噪声贡献。在每个实例中，过程和测量噪声都是不知道的。只有噪声统计数据是已知的。噪声统计数据是已知的。
可以把这些方程放到一个递归循环中，来估计状态如何演化，也可以估计状态分量的不确定性如何演化。
4.4 滤波环
从状态的最佳估计值x0/0和状态协方差P0/0开始，滤波器在不断的循环中执行这些步骤。
1 使用运动方程将状态传播到下一步
将协方差矩阵也传播出去。
下标符号k+1|k表示该量是在k+1步时从k步开始传播的最优估计，这个估计通常被称为先验估计。
然后预测更新时间的测量结果。
2 利用实际测量和预测测量的差值来修正更新时间的状态。修正需要计算卡尔曼增益。要做到这一点，首先要计算测量预测协方差（创新）。
然后卡尔曼滤波的增益为：
并由使用最优条件得出。
3 用测量值修正预测估计值。假设估计值是预测状态与测量值的线性组合。修正后的估计值使用下标符号，k+1|k+1.是由以下内容计算出来的
其中Kk+1为卡尔曼增益。修正后的状态通常被称为状态的后验估计，因为它是在包含测量之后得出的。
修正状态协方差矩阵
最后，可以根据修正后的状态来计算测量值。这不是对测量值的修正，而是对测量值的最佳估计，是基于状态的最佳估计。将其与实际测量值进行比较，就可以了解滤波器的性能。
此图总结了卡尔曼环的操作：
4.5 恒速模型
线性卡尔曼滤波器包含一个内置的线性恒速运动模型。另外，也可以指定线性运动的过渡矩阵。下一个时间步的状态更新是当前时间状态的线性函数。在这个滤波器中，测量值也是测量矩阵所描述的状态的线性函数。对于一个在三维空间中运动的物体，状态由x、y、z坐标中的位置和速度来描述。恒速运动的状态转换模型为
测量模型是状态向量的线性函数。最简单的情况是，测量值是状态的位置分量。
trackingKF 函数实例：
x = 5.3;
y = 3.6;
initialState = [x;0;y;0];
KF = trackingKF('MotionModel','2D Constant Velocity','State',initialState);
从恒定速度的轨迹中创建测量的位置：
vx = 0.2;
vy = 0.1;
T = 0.5;
pos = [0:vx*T:2;5:vy*T:6]';
预测并纠正物体的状态：
for k = 1:size(pos,1)
pstates(k,:) = predict(KF,T);
cstates(k,:) = correct(KF,pos(k,:));
end
绘制跟踪曲线图：
plot(pos(:,1),pos(:,2),'k.', pstates(:,1),pstates(:,3),'+', ...
cstates(:,1),cstates(:,3),'o')
xlabel('x [m]')
ylabel('y [m]')
grid
xt = [x-2 pos(1,1)+0.1 pos(end,1)+0.1];
yt = [y pos(1,2) pos(end,2)];
text(xt,yt,{'First measurement','First position','Last position'})
legend('Object position', 'Predicted position', 'Corrected position')
4.6 恒加速模型
线性卡尔曼滤波器包含一个内置的线性恒加速运动模型。另外，也可以指定恒加速度线性运动的过渡矩阵。线性加速度的过渡模型是
最简单的情况是，测量的是状态的位置分量。