线性卡尔曼滤波器- MATLAB & Simulink - Mat金宝apphWorks España - 金宝app,下载188bet金宝搏,金宝搏官方网站

线性卡尔曼滤波器

卡尔曼滤波器利用一系列的检测或测量来跟踪目标状态基于对象的运动模型的对象的。在运动模型中，状态是表示物体状态的数量集合，例如物体的位置、速度和加速度。物体运动模型是由物体状态的演变来定义的。

线性卡尔曼滤波器(trackingKF)是一种最优的递归算法，用于估计对象的状态，如果估计系统是线性和高斯的。如果运动模型和测量模型都是线性的，那么估计系统就是线性的。过滤器的工作原理是使用运动模型递归地预测物体状态，并使用测量值修正状态。

运动模型

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

考虑一个物体在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}] 一个$

在大多数情况下，运动模型并不能完全模拟对象的运动，您需要包含过程噪声补偿运动模型中的不确定性。对于等速模型，可以添加过程噪声作为加速项。

在这里,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} \frac{1}{2} T^{2} \\ T \end{matrix}] 一个 + ［ \begin{matrix} \frac{1}{2} T^{2} \\ T \end{matrix}] \tilde{v}$

在哪里x_{k+ 1}是离散时间的状态吗k+ 1,x_k是在早期离散时间的状态吗k。如果加上噪声，方程就会变得更加复杂，因为噪声的积分并不简单。有关如何从连续系统中获得离散过程噪声的详细信息，请参见[1]。

你可以将状态方程推广为:

$x_{k + 1} ＝ {一个}_{k} x_{k} + B_{k} u_{k} + G_{k} v_{k}$

在哪里一个_k状态转移矩阵是和吗B_k是控制矩阵。控制矩阵表示作用在物体上的任何已知力。v_k表示离散化过程噪声，服从均值0和协方差的高斯分布问_k。G_k为过程噪声增益矩阵。

测量模型

测量是你在一个系统中观察或测量的东西。测量依赖于状态向量，但通常与状态向量不同。例如，在雷达系统中，测量值可以是球坐标，如距离、方位角和仰角，而状态向量是笛卡尔位置和速度。线性卡尔曼滤波器假设测量值是状态向量的线性函数。要应用非线性测量模型，您可以选择使用扩展卡尔曼滤波器(trackingEKF)或无味卡尔曼滤光片(trackingUKF）.

您可以将线性测量表示为:

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

在这里,H_k测量矩阵是和吗w_k表示当前时间步长的测量噪声。对于最优滤波器，测量噪声必须为零均值高斯白噪声。假设测量噪声的协方差矩阵为R_k。

过滤循环

过滤器从状态的最佳估计开始x_{0 | 0}状态协方差P_{0 | 0}。过滤器在递归循环中执行这些步骤。

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

$x_{k + 1 | k} ＝ F_{k} x_{k | k} + B_{k} u_{k} 。$

同时传播协方差矩阵:

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

下标符号k+ 1 |k表示对应的数量是在的估计值k+1步从第一步开始传播k。这个估计通常被称为先天的估计。的预测测量值k+1步是

$z_{k + 1 | k} ＝ H_{k + 1} x_{k + 1 | k}$
使用实际测量值与预测测量值之间的差值来纠正点处的状态k+ 1的步骤。为了纠正这种状态，滤波器必须计算卡尔曼增益。首先，滤波器计算测量预测协方差(创新)为:

${年代}_{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}$
滤波器通过测量来校正预测的估计。使用测量校正后的估计z_{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}^{T}$

该图总结了卡尔曼循环操作。一旦初始化，卡尔曼滤波器在预测和校正之间循环，直到模拟结束。

卡尔曼滤波方程

内置运动模型`trackingKF`

当您只需要使用标准的1-D、2-D或3-D恒定速度或恒定加速度运动模型时，可以指定MotionModel的属性trackingKF作为其中之一:

“一维恒速”
“一维恒加速度”
二维等速
“二维恒定加速度”
“3D等速”
“3D恒加速”

要自定义自己的运动模型，请指定MotionModel财产“自定义”，然后指定状态转移矩阵StateTransitionModel属性。

对于三维等速模型，状态方程为:

$［ \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 + 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}]$

示例:使用估算二维目标状态`trackingKF`

打开实时脚本

初始化估计模型

为假设在二维中移动的目标指定初始位置和速度。仿真时间为20秒，采样时间为0.2秒。

rng (2021);%用于重复结果Dt = 0.2;%秒simTime = 20;%秒tspan = 0:dt:simTime;trueInitialState = [30;2;40;2);% (x, vx; y; v)processNoise = diag([0;1;0;1);%过程噪声矩阵

假设目标测量由其位置状态组成，创建测量噪声协方差矩阵。

measureNoise = diag([4 4]);%测量噪声矩阵

该矩阵在x和y方向上指定了2米的标准差。

预分配变量以保存估计结果。

numSteps = length(tspan);trueStates = NaN(4,numSteps);trueStates(:，1) = trueInitialState;estimateStates = NaN(size(trueStates));

获取真实状态和测量值

传播匀速模型，产生带噪声的测量结果。

F = [1 dt 0 0;0 1 0 0;0 0 1 dt;0 0 0 1];H = [1 0 0 0;0 0 1 0];为i = 2:length(tspan) trueStates(:，i) = F*trueStates(:，i-1) + sqrt(processNoise)*randn(4,1);结束measurements = H*trueStates + sqrt(measureNoise)*randn(2,numSteps);

画出真实的轨迹和测量值。

图绘制(trueStates (1, 1), trueStates (3,1),“r *”DisplayName =“真正的初始”)举行在情节(trueStates (1:), trueStates (3:)“r”DisplayName =“真相”)情节(测量(1:),测量(2:)“kx”DisplayName =“测量”)包含(“x”(m)) ylabel (“y (m)”)轴图像

图包含一个轴对象。具有xlabel x (m)， ylabel y (m)的axes对象包含3个类型为line的对象。其中一条或多条线仅使用标记显示其值。这些对象表示True Initial, True, Measurements。

初始化线性卡尔曼滤波器

初始化状态为的过滤器[40;0;160;0]，这与真实的初始状态相去甚远。通常，您可以使用初始度量来构造初始状态为(测量(1,1);0;测量(2,1);0]。在这里，您使用一个错误的初始状态，这使您能够测试过滤器是否能够快速收敛于事实。

filter = trackingKF(MotionModel=二维等速状态= [40;0;160; 0],．．．MeasurementModel = H, MeasurementNoise = measureNoise)

filter = trackingKF with properties: State: [4x1 double] StateCovariance: [4x4 double] MotionModel: '2D Constant Velocity' ProcessNoise: [2x2 double] MeasurementModel: [2x4 double] MeasurementNoise: [2x2 double] MaxNumOOSMSteps: 0 EnableSmoothing: 0

estimateStates(:，1) = filter.State;

运行线性卡尔曼滤波并显示结果

方法来运行筛选器预测和正确的对象的功能。从结果来看，这些估计很快就接近事实。事实上，线性卡尔曼滤波器具有指数收敛速度。

为i=2:length(tspan) predict(filter,dt) estimateStates(:，i) = correct(filter,measurements(:，i))结束情节(estimateStates (1, 1), estimateStates (3,1),“g *”DisplayName =“初步估计”)情节(estimateStates (1:), estimateStates (3:)“g”DisplayName =“估计”传奇(位置=“东南”）

图包含一个轴对象。具有xlabel x (m)， ylabel y (m)的axes对象包含5个类型为line的对象。一条或多条行仅使用标记显示其值。这些对象表示True Initial、True、Measurements、Initial estimate、estimate。