线性卡尔曼滤波器- MATLAB和Simulink MathWor金宝appks日本 - 金宝app,下载188bet金宝搏,金宝搏官方网站

线性卡尔曼过滤器

当您使用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}] \tilde{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}$

最后，您可以基于校正状态计算测量。这不是对测量的校正，而是最好估计测量将基于状态的最佳估计。将其与实际测量进行比较，为您提供了滤波器性能的指示。

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

卡尔曼方程式

恒定速度模型

线性卡尔曼滤波器包含内置线性恒定速度运动模型。或者，您可以指定用于线性运动的转换矩阵。下次步骤的状态更新是当前状态的线性函数。在该滤波器中，测量结果也是由测量矩阵描述的状态的线性函数。对于在3-D空间中移动的物体，状态由位置和速度描述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