Linear Kalman筛选器 - Matlab＆Simuli万博1manbetxnk - 万博1manbetx,s manbetx 845,万博尤文图斯

线性卡尔曼滤波器

当您使用Kalman筛选器来跟踪对象时，使用一系列检测或测量来构造对象运动的模型。对象运动由对象状态的演变定义。卡尔曼滤波器是用于估计对象的轨道的最佳递归算法。过滤器是递归的，因为它使用先前状态更新当前状态，使用可能在间隔中进行的测量值。卡尔曼滤波器包含这些新测量，以保持状态估计尽可能准确。过滤器是最佳的，因为它最大限度地减少了状态的平均方误差。您可以使用过滤器来预测未来状态或估计当前状态或过去的状态。

状态方程式

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

从牛顿方程开始，让对象移动X- 在恒定加速度下，将这些方程转换为空间状态。

$\begin{array}{l} m \ddot{X} = F \\ \ddot{X} = \frac{F}{m} = 一种 \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}] 一种$

当您有信心遵循这种类型的运动时，使用线性动态模型。有时该模型包括过程噪声反映运动模型中的不确定性。在这种情况下，牛顿方程有一个额外的术语。

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.} 你_{K.} + {V.}_{K.}$

F_K.是状态转换矩阵和G_K.是控制矩阵。控制矩阵考虑了作用在物体上的任何已知力。给出了这两种矩阵。最后一个术语表示动态模型的噪声样随机扰动。假设噪声是零均值高斯白噪声。

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

测量模型

测量是您观察到您的系统的内容。测量依赖于状态向量，但并不总是与状态向量相同。例如，在雷达系统中，测量可以是球形坐标，例如范围，方位角和升降，而状态向量是笛卡尔位置和速度。对于线性卡尔曼滤波器，测量结果始终是状态向量的线性函数，从而排除球形坐标。要使用球形坐标，请使用扩展的卡尔曼滤波器。

测量模型假设随时实际测量与当前状态有关

${Z.}_{K.} = H_{K.} X_{K.} + {W.}_{K.}$

W._K.表示当前时间步骤的测量噪声。测量噪声也是具有协方差矩阵的零均值白色高斯噪声问：由此描述问：_K.= e [n_K.N_K.^T.]。

线性卡尔曼滤波器方程

没有噪声，动态方程是

$X_{K. + 1} = F_{K.} X_{K.} + G_{K.} 你_{K.} 。$

同样，测量模型没有测量噪声贡献。在每种情况下，该过程和测量噪声尚不清楚。只知道噪声统计信息。这

${Z.}_{K.} = H_{K.} X_{K.}$

您可以将这些方程式放入递归循环中以估计国家的发展方式以及状态分量中的不确定性如何发展。

过滤循环

从最佳估计开始，X_0/0，国家协方差，P._0/0。过滤器在持续循环中执行这些步骤。

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

$X_{K. + 1 | K.} = F_{K.} X_{K. | K.} + G_{K.} 你_{K.} 。$

传播协方差矩阵。

${P.}_{K. + 1 | K.} = F_{K.} {P.}_{K. | K.} F_{K.}^{T.} + {问：}_{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}$

最后，您可以基于校正状态计算测量。这不是对测量的校正，而是最好估计测量基于状态的最佳估计。将其与实际测量相比，您可以指示过滤器的性能。

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

恒定速度模型

线性卡尔曼滤波器包含内置线性恒流运动模型。或者，您可以指定用于线性运动的转换矩阵。下次步骤的状态更新是当前状态的线性函数。在该滤波器中，测量结果也是由测量矩阵描述的状态的线性函数。对于在三维空间中移动的物体，状态通过位置和速度描述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} \\ {一种}_{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} 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.} \\ {一种}_{X 那 K.} \\ y_{K.} \\ {V.}_{y 那 K.} \\ {一种}_{y 那 K.} \\ {Z.}_{K.} \\ {V.}_{Z. 那 K.} \\ {一种}_{y 那 K.} \end{matrix}]$

也可以看看

对象

trackingkf.