卡尔曼工具箱的安装与使用_MATLAB开发自动驾驶第十二课-线性卡尔曼滤波

红太狼 2022-10-26 12:45 113阅读 0赞

![22d366bb6de2e8ba04da1251b9716b8a.png][]

当使用卡尔曼滤波器跟踪物体时，会使用一连串的检测或测量来构建物体运动的模型。物体运动是由物体状态的演变来定义的。卡尔曼滤波器是一种最佳的、递归的算法，用于估计物体的运动轨迹。滤波器之所以是递归的，是因为它使用之前的状态，使用可能在该区间内进行的测量来更新当前状态。卡尔曼滤波器将这些新的测量值纳入其中，以保持状态估计的尽可能准确。该滤波器是最佳的，因为它使状态的均方误差最小化。可以使用该滤波器来预测未来的状态或估计当前状态或过去的状态。

### 状态方程 ###

对于自动驾驶工具箱中跟踪的大多数类型的物体，状态向量由一维、二维或三维位置和速度组成。

从物体在x方向以恒定加速度运动的牛顿方程开始，将这些方程转换为空间状态形式。

![202d2e5d787fad2c817651cfe299bde7.png][]

如果定义状态作为：

![c5065c3f11d1ada2c53cc48c18cbbbde.png][]

可以将牛顿定律写成如下状态方程：

![006f40d035772c2622ef37ece8f31286.png][]

当你对物体遵循这种类型的运动有信心时，你会使用线性动态模型。有时，模型包括过程噪声，以反映运动模型的不确定性。在这种情况下，牛顿方程有一个附加项。

![a30c5954af58ad0a229b0071d2b9c78b.png][]

Vk是加速度的未知噪声扰动。只有噪声的统计量是已知的。假设它是零均值的高斯白噪声。

可以将这种类型的方程扩展到一个以上的维度。在二维中，方程的形式为：

![58dd72cf3dccde8386e3f905f8ef13aa.png][]

右侧的4×4矩阵是状态转换模型矩阵。对于独立的x和y运动，这个矩阵是块对角线。

当你过渡到离散时间时，你会在时间间隔的长度上整合运动方程。在离散形式中，对于T的样本区间，状态表示变成了：

![624192d16463809980336fc87358c300.png][]

量xk+1是离散时间k+1时的状态，xk是更早的离散时间k时的状态。如果加入噪声，方程就会变得更复杂，因为噪声的积分并不简单。

状态方程可以概括为：

![43a867212a6342903eca71aef8f8f572.png][]

Fk是状态转换矩阵，Gk是控制矩阵。控制矩阵考虑了作用在物体上的任何已知力。这两个矩阵均已给出。最后一个项代表动态模型的类似噪声的随机扰动。噪声被假定为零均值高斯白噪声。

有输入噪声的连续时间系统由线性随机微分方程描述。有输入噪声的离散时间系统用线性随机微分方程描述。状态空间表示法是一个物理系统的数学模型，其中输入、输出和状态变量由一阶耦合方程相关。

### 测量模型 ###

测量是对系统的观察。测量值取决于状态矢量，但并不总是与状态矢量相同。例如，在雷达系统中，测量值可以是球面坐标，如距离、方位角和海拔，而状态矢量是笛卡尔位置和速度。对于线性卡尔曼滤波器，测量值总是状态矢量的线性函数，排除了球面坐标。要使用球面坐标，请使用扩展卡尔曼滤波器。

测量模型假设任何时候的实际测量值都与当前状态有关，其方法是：

*zk* = *Hkxk* \+ *wk*

wk代表当前时间步的测量噪声。测量噪声也是零均值白高斯噪声，其协方差矩阵Q由以下方式描述：

*Qk = E\[n n T\]*.

### 线性卡尔曼滤波等式 ###

如果没有噪声，则卡尔曼滤波的等式应该为：

*xk* \+ 1 = *Fkxk* \+ *Gkuk* .

同样，测量模型也没有测量噪声贡献。在每个实例中，过程和测量噪声都是不知道的。只有噪声统计数据是已知的。噪声统计数据是已知的。

*zk* = *Hkxk*

你可以把这些方程放到一个递归循环中，来估计状态如何演化，也可以估计状态分量的不确定性如何演化。

### 滤波环 ###

从状态的最佳估计值x0/0和状态协方差P0/0开始，滤波器在不断的循环中执行这些步骤。

1 使用运动方程将状态传播到下一步

将协方差矩阵也传播出去。

![0aa2f961ea54500225d6e04858481b0d.png][]

下标符号k+1|k表示该量是在k+1步时从k步开始传播的最优估计，这个估计通常被称为先验估计。然后预测更新时间的测量结果。

2 利用实际测量和预测测量的差值来修正更新时间的状态。修正需要计算卡尔曼增益。要做到这一点，首先要计算测量预测协方差：

![ad2fa1488dad66e3c6818637a59d9e39.png][]

然后卡尔曼滤波的增益为：

![9b0116ade7c2a50a423d728949838074.png][]

并由使用最优条件得出。

3用测量值修正预测估计值。假设估计值是预测状态与测量值的线性组合。修正后的估计值使用下标符号，k+1|k+1.是由以下内容计算出来的

![2c2208994c634d3c83d7096659503358.png][]

![df30da1c380156d00887aa063024aba9.png][]

其中Kk+1为卡尔曼增益。修正后的状态通常被称为状态的后验估计，因为它是在包含测量之后得出的。

修正状态协方差矩阵

![f0d9d00f02324e50331244bccdee6bf8.png][]

最后，可以根据修正后的状态来计算测量值。这不是对测量值的修正，而是对测量值的最佳估计，是基于状态的最佳估计。将其与实际测量值进行比较，您就可以了解滤波器的性能。

此图总结了卡尔曼计算流程：

![bb5023444c84a57576c6a92415bce343.png][]

### 恒速模型 ###

线性卡尔曼滤波器包含一个内置的线性恒速运动模型。另外，您也可以指定线性运动的过渡矩阵。下一个时间步的状态更新是当前时间状态的线性函数。在这个滤波器中，测量值也是测量矩阵所描述的状态的线性函数。对于一个在三维空间中运动的物体，状态由x、y、z坐标中的位置和速度来描述。恒速运动的状态转换模型为

![f94880fa068f9276c2ef0339a40b488e.png][]

测量模型是状态向量的线性函数。最简单的情况是，测量值是状态的位置分量。

![e2910608bc1474cb79e9f5a55eac0837.png][]

### 恒加速模型 ###

线性卡尔曼滤波器包含一个内置的线性恒加速运动模型。另外，您也可以指定恒加速度线性运动的过渡矩阵。线性加速度的过渡模型是

![f658700aecb8bb5363e88832cbdfddd9.png][]

最简单的情况是，测量的是状态的位置分量。

![64e6c19ed8ece72c7cbde84afd0ffd24.png][]

[22d366bb6de2e8ba04da1251b9716b8a.png]: /images/20221024/129dcde9cc744168b38e1a1b089013ee.png
[202d2e5d787fad2c817651cfe299bde7.png]: /images/20221024/109d8e557be44f3d9946dca7f7ee891c.png
[c5065c3f11d1ada2c53cc48c18cbbbde.png]: /images/20221024/1ecd33b91e0641b18f0c87a08ae138f7.png
[006f40d035772c2622ef37ece8f31286.png]: /images/20221024/6995c93f48044ab9bb0e35fa98c91d99.png
[a30c5954af58ad0a229b0071d2b9c78b.png]: /images/20221024/3de202c2890e46f5b9489996848d6a5d.png
[58dd72cf3dccde8386e3f905f8ef13aa.png]: /images/20221024/a3cc522cb0064627939837397c6d30c4.png
[624192d16463809980336fc87358c300.png]: /images/20221024/b7227b26594645d6ab203190788f766d.png
[43a867212a6342903eca71aef8f8f572.png]: /images/20221024/29218b47a87f4ff7b7ad808b3caaa372.png
[0aa2f961ea54500225d6e04858481b0d.png]: /images/20221024/b727139ea57f49e0b8f9a4e1da969b0f.png
[ad2fa1488dad66e3c6818637a59d9e39.png]: /images/20221024/8ae818f3a02745db92db51c67e6a9675.png
[9b0116ade7c2a50a423d728949838074.png]: /images/20221024/02f8f61810b8440391d5bd077da3a95d.png
[2c2208994c634d3c83d7096659503358.png]: /images/20221024/e9ad5342fd8b42cbb99ebe83f9a82801.png
[df30da1c380156d00887aa063024aba9.png]: /images/20221024/d325a5f388f6428980d7020c875765b9.png
[f0d9d00f02324e50331244bccdee6bf8.png]: /images/20221024/e7a0917e2fbf441caa82c85f522e5e06.png
[bb5023444c84a57576c6a92415bce343.png]: /images/20221024/1572124968fa4d0e8feb8ed34fb25051.png
[f94880fa068f9276c2ef0339a40b488e.png]: /images/20221024/b0d73c24287a42a99713cd4f5fc854c9.png
[e2910608bc1474cb79e9f5a55eac0837.png]: /images/20221024/04e43b8d4afa46a4a587609b9db2699b.png
[f658700aecb8bb5363e88832cbdfddd9.png]: /images/20221024/8964979f7eea4efda9e8cc9936e823c5.png
[64e6c19ed8ece72c7cbde84afd0ffd24.png]: /images/20221024/3df71b85b353432bbf8f1ebc13f55cfb.png