线性卡尔曼滤波器

当您使用卡尔曼滤波器跟踪对象时，您使用一系列的检测或测量来构建对象运动的模型。物体的运动是由物体状态的演变来定义的。卡尔曼滤波是一种估计目标轨迹的递归优化算法。过滤器是递归的，因为它使用前一个状态更新当前状态，使用可能在间隔期间进行的测量。一个卡尔曼滤波器合并这些新的测量数据，以保持状态估计尽可能准确。该滤波器是最优的，因为它最小化了状态的均方误差。您可以使用筛选器来预测未来状态或估计当前状态或过去状态。

状态方程

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

用牛顿方程开始的对象在运动x将这些方程转换成空间态形式。

$\begin{array}{l} 米 \ddot{x} = f \\ \ddot{x} = \frac{f}{米} = 一个 \end{array}$

如果你把状态定义为

$\begin{array}{l} x_{1} = x \\ x_{2} = \dot{x}, \end{array}$

你可以写牛顿的状态空间形式的法律。

$\frac{d}{d t} (\begin{matrix} x_{1} \\ x_{2} \end{matrix}] = (\begin{matrix} 0 & 1 \\ 0 & 0 \end{matrix}] (\begin{matrix} x_{1} \\ x_{2} \end{matrix}] + (\begin{matrix} 0 \\ 1 \end{matrix}] 一个$

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

$\frac{d}{d t} (\begin{matrix} x_{1} \\ x_{2} \end{matrix}] = (\begin{matrix} 0 & 1 \\ 0 & 0 \end{matrix}] (\begin{matrix} x_{1} \\ x_{2} \end{matrix}] + (\begin{matrix} 0 \\ 1 \end{matrix}] 一个 + (\begin{matrix} 0 \\ 1 \end{matrix}] v_{k}$

v_k是加速度的未知噪声扰动。只有噪声的统计数据是已知的。它被认为是零均值高斯白噪声。

可以将这种类型的方程扩展到多个维度。在二维中，方程有这种形式

$\frac{d}{d t} (\begin{matrix} x_{1} \\ x_{2} \\ y_{1} \\ y_{2} \end{matrix}] = (\begin{matrix} 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \end{matrix}] (\begin{matrix} x_{1} \\ x_{2} \\ y_{1} \\ y_{2} \end{matrix}] + (\begin{matrix} 0 \\ {一个}_{x} \\ 0 \\ {一个}_{y} \end{matrix}] + (\begin{matrix} 0 \\ v_{x} \\ 0 \\ v_{y} \end{matrix}]$

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

当你转换到离散时间，你在时间间隔的长度上积分运动方程。在离散形式下，对于一个采样区间T中，状态表示变

$(\begin{matrix} x_{1, k + 1} \\ x_{2, k + 1} \end{matrix}] = (\begin{matrix} 1 & T \\ 0 & 1 \end{matrix}] (\begin{matrix} x_{1, k} \\ x_{2, k} \end{matrix}] + (\begin{matrix} 0 \\ T \end{matrix}] 一个 + (\begin{matrix} 0 \\ 1 \end{matrix}] \overset{〜}{v}$

数量x_{K + 1}是在离散的时间的状态K + 1和x_k在较早的离散时间的状态，k。如果包含噪声，方程会变得更复杂，因为噪声的积分并不简单。

状态方程可以推广到

$x_{k + 1} = F_{k} x_{k} + G_{k} u_{k} + v_{k}$

F_k是状态转移矩阵和G_k为控制矩阵。控制矩阵考虑作用在物体上的任何已知力。这两个矩阵都给出了。最后一项表示动态模型的类噪声随机扰动。假设噪声为零均值高斯白噪声。

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

计量模型

测量你观察你的系统是什么。测量取决于状态向量，但并不总是一样的状态向量。例如，在雷达系统中，测量可以是球面坐标诸如距离，方位和高程，而状态向量是笛卡尔位置和速度。对于线性卡尔曼滤波器，测量总是状态向量的线性函数，排除球形坐标。要使用球面坐标，使用扩展卡尔曼滤波。

测量模型假定在任何时间的实际测量是通过与所述当前状态

$z_{k} = H_{k} x_{k} + w_{k}$

w_k表示当前时间步长的测量噪声。测量噪声也是具有协方差矩阵的零均值高斯白噪声问所描述的问_k= E [N_kn_k^T]。

线性卡尔曼滤波的方程

无噪音，动态方程

$x_{k + 1} = F_{k} x_{k} + G_{k} u_{k} 。$

同样地，测量模型没有测量噪声的贡献。在每个实例中，过程和测量噪声是未知的。只有噪声统计是已知的。该

$z_{k} = H_{k} x_{k}$

你可以把这些方程为递归循环来估计状态演变以及如何在不确定性的状态组件如何演变。

过滤循环

先从状态的最佳估计，x_0/0，国家协方差，P_0/0。过滤器在一个连续的循环中执行这些步骤。

使用运动方程将状态传播到下一个步骤。

$x_{k + 1 | k} = F_{k} x_{k | k} + G_{k} u_{k} 。$

传播协方差矩阵为好。

$P_{k + 1 | k} = F_{k} P_{k | k} F_{k}^{T} + 问_{k} 。$

下标符号K + 1 | K表明，该数量为最优估计值K + 1从step传播的stepk。这个估计通常被称为先验估计。
然后在预测更新的时间测量。

$z_{k + 1 | k} = H_{k + 1} x_{k + 1 | k}$
使用实际测量和预测的测量值之间的差异在更新的时间来纠正状态。矫正需要计算卡尔曼增益。要做到这一点，首先计算测量预测协方差（创新）

${年代}_{k + 1} = H_{k + 1} P_{k + 1 | k} H_{k + 1}^{T} + R_{k + 1}$

然后卡尔曼增益

$K_{k + 1} = P_{k + 1 | k} H_{k + 1}^{T} {年代}_{k + 1}^{- 1}$

并且从使用最优性条件的。
用测量法修正预测的估计值。假设估计值是预测状态和测量值的线性组合。修正后的估计值使用下标符号，K + 1 | K + 1。计算从

$x_{k + 1 | k + 1} = x_{k + 1 | k} + K_{k + 1} (z_{k + 1} - z_{k + 1 | k})$

在哪里K_{K + 1}是卡尔曼增益。修正状态通常被称为后验因为包含在测量之后它源自的状态的估计。
更正状态协方差矩阵

$P_{k + 1 | k + 1} = P_{k + 1 | k} - K_{k + 1} {年代}_{k + 1} {K^{'}}_{k + 1}$

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

该图总结了卡尔曼循环操作。

恒定的速度模型

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

$(\begin{matrix} x_{k + 1} \\ v_{x, k + 1} \\ y_{k + 1} \\ v_{y, k + 1} \\ z_{k + 1} \\ v_{z, k + 1} \end{matrix}] = (\begin{matrix} 1 & T & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & T & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & T \\ 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}] (\begin{matrix} x_{k} \\ v_{x, k} \\ y_{k} \\ v_{y, k} \\ z_{k} \\ v_{z, k} \end{matrix}]$

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

$(\begin{matrix} 米_{x, k} \\ 米_{y, k} \\ 米_{z, k} \end{matrix}] = (\begin{matrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \end{matrix}] (\begin{matrix} x_{k} \\ v_{x, k} \\ y_{k} \\ v_{y, k} \\ z_{k} \\ v_{z, k} \end{matrix}]$

恒定加速度模型

线性卡尔曼滤波器包含一个内置的线性恒定加速度运动模型。可替换地，可以指定用于恒定加速度直线运动的转换矩阵。线性加速度的过渡模式是

$(\begin{matrix} x_{k + 1} \\ v_{x, k + 1} \\ {一个}_{x, k + 1} \\ y_{k + 1} \\ v_{y, k + 1} \\ {一个}_{y, k + 1} \\ z_{k + 1} \\ v_{z, k + 1} \\ {一个}_{z, k + 1} \end{matrix}] = (\begin{matrix} 1 & T & \frac{1}{2} T^{2} & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & T & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & T & \frac{1}{2} T^{2} & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & T & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & T & \frac{1}{2} T^{2} \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & T \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}] (\begin{matrix} x_{k} \\ v_{x, k} \\ {一个}_{x, k} \\ y_{k} \\ v_{y, k} \\ {一个}_{y, k} \\ z_{k} \\ v_{z, k} \\ {一个}_{z, k} \end{matrix}]$

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

$(\begin{matrix} 米_{x, k} \\ 米_{y, k} \\ 米_{z, k} \end{matrix}] = (\begin{matrix} 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \end{matrix}] (\begin{matrix} x_{k} \\ v_{x, k} \\ {一个}_{x, k} \\ y_{k} \\ v_{y, k} \\ {一个}_{y, k} \\ z_{k} \\ v_{z, k} \\ {一个}_{y, k} \end{matrix}]$

另请参阅

对象

trackingKF