无人驾驶之MATLAB无人驾驶工具箱学习(2)
本文主要为MATLAB2018a无人驾驶工具箱ADST用户指导手册一部分学习笔记。大部分为翻译手册而来。
3. 跟踪与信息融合
- 线性卡尔曼滤波(推荐一篇比较好的卡尔曼滤波科普博文)
- 扩展卡尔曼滤波
线性卡尔曼滤波：
当利用卡尔曼滤波器进行轨迹跟踪时，需要用检测或测量值去构建物体的运动模型。物体运动用物体状态的演化进行定义。卡尔曼滤波器是估计物体轨迹的最佳递归算法。该滤波器用先前状态以及该时间间隔内的测量值更新当前状态。卡尔曼滤波器利用这些新的测量值使得状态估计尽可能准确，同时最小化状态的均方误差以达到最优。可以利用该滤波器预测未来状态或者估计当前/以前状态。
状态方程
对于自动驾驶工具箱中的大多数物体跟踪类型，状态向量包含1-、2-、3-维位置和速度。对于一个沿着x轴方向运动并保持恒定加速度的物体，可以将其牛顿方程转化为相空间形式(space-state form)：
定义状态：
牛顿定律的状态空间方程为：
如果物体的运动形式符合线性动力学模型，有时该模型会包含反应运动模型不确定性的process noise(过程噪声？)。在这种情况下，牛顿方程需要增加一项。
为加速度未知噪声扰动（perturbation）。只有噪声的统计分布已知，为零均值高斯白噪声。
可以将上式扩展。二维的情况如下：
右侧4×4矩阵即为状态转移矩阵。
、
方向上的运动相互独立，矩阵程分块对角阵。
如果变换为离散时间变换，在时间间隔对运动方程进行积分。对于离散形式，对于一个采样周期
，状态表示变为：
为离散时刻
时对应的状态。如果包含噪声，该式将变得更加复杂，因为噪声积分不是显式的。状态方程可以表示为
为状态转移矩阵，
为控制矩阵。控制矩阵包含一切作用在物体上的已知力。所有矩阵都必须已知。最后一项代表动力学模型的类型噪声的随机扰动。该噪声假设为零矩阵高斯白噪声。
有输入噪声的连续\离散时间系统用线性随机微分方程进行描述。状态空间表示是物理系统的数学模型，该模型中输入、输出、状态变量为一阶耦合方程。
测量模型(Measurement Models)
测量依赖于持续变化的状态向量。例如，在雷达系统中，测量可以为球面坐标(如范围(range)、方位(azimuth)和高度(elevation))，状态向量为笛卡尔位置和速度。对于线性卡尔曼滤波器，测量值一直为状态向量的线性函数，排除球面坐标。如果要用球面坐标，需要用扩展卡尔曼滤波器。
测量模型假设为任何时刻基于当前状态的实际测量值，
表示当前时间补偿下的测量噪声。测量噪声为零均值高斯白噪声，其协方差矩阵为![Q_k=E[n_k{n_k}^T]](http://img.e-com-net.com/image/info8/e8a94c2c7db44be7b5948c305b67ab88.gif){kind=small}
.
线性卡尔曼滤波器方程
在没有噪声的情况下，动力学方程为
同样的，测量模型也没有噪声成分。在每一种情景下，过程噪声和测量噪声都是未知的，只有噪声分布已知。
可以将这些方程放置到循环中，估计状态及状态中的不确定性如何转移。
滤波器循环
在最佳估计值处开始，此时状态为
，协方差为
.在一个连续循环中，滤波器的步骤如下
1.用运动方程将状态转移到下一时刻。
变换协方差矩阵：
下标
表示该量为从
转移到时的最佳估计。该估计通常被称为先验估计。
然后，在更新时刻估计测量：
2.用实际测量值与预测测量值之间的偏差修改更新时刻的状态。修正需要计算卡尔曼增益。
首先计算测量值的预测协方差：
卡尔曼增益为
其来自于一个最优化条件。
3.用测量值修正预测估计。假设估计为预测状态和测量之间的线性组合。修正之后的估计用下标
,用下式进行计算：
其中
为卡尔曼增益。修正状态被称作后验估计。
修正协方差矩阵：
最后，可以计算一个机遇当前修正状态的测量值。这不是对测量的修正，而是基于当前最佳估计状态的测量的最佳估计。比较该测量估计值与实际测量值，可以估计滤波器的性能。
卡尔曼滤波器工作过程如下：
匀速模型
在该滤波器中，测量也是当前时刻状态的线性函数，用测量矩阵进行表示。对于在三维空间中运动的物体，状态向量用x-、y-、z-坐标及速度进行表示。匀速运动的状态转移模型为：
测量模型为状态向量的线性方程。最简单的情况，测量值即为状态中的位置。
匀加速模型
匀加速模型的转移模型为：
最简单的情况，测量值即为状态中的位置坐标：
扩展卡尔曼滤波器
当物体运动遵循非线性状态方程或测量为状态的非线性函数时，采用扩展卡尔曼滤波器。比如，当状态或测量值通过球面坐标系得到时，比如方位角、高度、赋值等。
状态更新模型
扩展卡尔曼滤波器线性化状态方程。更新后的状态和协方差矩阵与前一时刻的状态和协方差矩阵保持线性关系。但是，线性卡尔曼滤波器的状态转移矩阵用状态方程的雅克比矩阵替代。雅克比矩阵不是恒定的，而是依赖于状态自身及时间。为了利用扩展卡尔曼滤波器，需要确定状态转移函数及其雅克比矩阵。
假设预测状态与先前状态、控制、噪声和时间的函数关系可以用解析表达式表示。
预测状态关于先前状态的雅克比即为
预测状态关于噪声的雅克比即为
当输入噪声在状态更新方程中为线性时，该函数可以简化为：
此时：
测量模型
在扩展卡尔曼滤波器中，测量可以为状态和测量噪声的非线性函数。
测量关于状态的雅克比为
测量关于噪声的雅克比即为
同样，当噪声在测量方程为中线性时，函数可以简化为：
此时：
扩展卡尔曼滤波器循环
扩展卡尔曼滤波器的序号与线性卡尔曼滤波器基本相同：
- 状态转移矩阵用状态雅克比矩阵替代。
- 测量矩阵用适当的雅克比矩阵进行替代。
参考资料：
Automated Driving System Toolbox™ User’s Guide.