线性卡尔曼滤波器-MATLAB和Simulink-MathWork万博1manbetxs瑞士 - 万博1manbetx,s manbetx 845,万博尤文图斯

线性卡尔曼滤波器

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

状态方程

对于工具箱中跟踪的大多数类型的对象，状态矢量由一、二或三维位置和速度组成。

从牛顿方程开始，物体在x-并将这些方程转化为空间状态形式。

$\begin{array}{l} M \ddot{x} = F \\ \ddot{x} = \frac{F}{M} = A. \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}] A.$

如果确信对象遵循此类型的运动，则使用线性动力学模型。有时模型包括过程噪声以反映运动模型中的不确定性。在这种情况下，牛顿方程还有一个附加项。

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 \\ {A.}_{x} \\ 0 \\ {A.}_{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}] A. + [\begin{matrix} 0 \\ 1. \end{matrix}] \tilde{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表示当前时间步的测量噪声。测量噪声也是具有协方差矩阵的零均值高斯白噪声Q描述Q_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} + Q_{K} ．$

下标符号k + 1 | k表示数量是当前的最佳估计值k + 1步骤从步骤传播K．这个估计通常被称为先天的估计。
然后在更新时间预测测量结果。

$Z_{K + 1. | K} = H_{K + 1.} x_{K + 1. | K}$
使用实际测量和预测测量的差值来修正更新时间的状态。校正需要计算卡尔曼增益。要做到这一点，首先计算测量预测协方差(创新)

$s_{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} s_{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.} s_{K + 1.} {K^{'}}_{K + 1.}$

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

此图总结了Kalman循环操作。

卡尔曼方程

等速模型

线性卡尔曼滤波器包含内置的线性等速运动模型。或者，可以为线性运动指定过渡矩阵。下一时间步的状态更新是当前状态的线性函数。在该滤波器中，测量值也是由测量矩阵描述的状态的线性函数。对于在三维空间中运动的物体，其状态由空间中的位置和速度来描述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} M_{x, K} \\ M_{Y, K} \\ M_{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.} \\ {A.}_{x, K + 1.} \\ Y_{K + 1.} \\ v_{Y, K + 1.} \\ {A.}_{Y, K + 1.} \\ Z_{K + 1.} \\ v_{Z, K + 1.} \\ {A.}_{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} \\ {A.}_{x, K} \\ Y_{K} \\ v_{Y, K} \\ {A.}_{Y, K} \\ Z_{K} \\ v_{Z, K} \\ {A.}_{Z, K} \end{matrix}]$

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

$[\begin{matrix} M_{x, K} \\ M_{Y, K} \\ M_{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} \\ {A.}_{x, K} \\ Y_{K} \\ v_{Y, K} \\ {A.}_{Y, K} \\ Z_{K} \\ v_{Z, K} \\ {A.}_{Y, K} \end{matrix}]$

另见

物体

trackingKF