线性卡尔曼滤波器,MATLAB仿真软件金宝app - 金宝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}] 一个$

在大多数情况下,运动模型并不完全模型运动的一个对象,你需要包括过程噪声补偿运动模型的不确定性。恒定的速度模型,您可以添加过程噪声作为一个加速度。

$\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} \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`

当你只需要使用标准的一维,二维,三维恒定速度或加速度恒定运动模型,您可以指定MotionModel的属性trackingKF作为其中的一个:

“一维恒定速度”
“一维恒定加速度”
“二维恒定速度”
“二维恒定加速度”
“3 d恒定速度”
“3 d恒定加速度”

定制你自己的运动模型,指定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 =诊断接头([0;1;0;1);%过程噪声矩阵

创建一个测量噪声协方差矩阵,假设目标测量包括它的位置状态。

measureNoise =诊断接头(4 [4]);%测量噪声矩阵

2米的矩阵指定一个标准偏差在x和y方向。

Preallocate变量保存的评估结果。

numSteps =长度(tspan);numSteps trueStates =南(4日);trueStates (: 1) = trueInitialState;estimateStates =南(大小(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);为我= 2:长度(tspan) trueStates (:, i) = F * trueStates(张:,)+ sqrt (processNoise) * randn (4,1);结束测量= H * trueStates +√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)”)轴图像

图包含一个坐标轴对象。坐标轴对象包含x (m), ylabel y (m)包含3线类型的对象。一个或多个行显示的值只使用这些对象标记代表真正的初始,真理,测量。

初始化线性卡尔曼滤波器

初始化滤波器的状态[40;0;160;0],这是远离真实的初始状态。通常,您可以使用初始计量来构造一个初始状态(测量(1,1);0;测量(2,1);0]。这里,我们使用一个错误的初始状态,这使您能够测试过滤器可以快速收敛的真理。

过滤器= trackingKF (MotionModel =“二维恒定速度”状态= [40;0;160;0],…MeasurementModel = H, MeasurementNoise = measureNoise)

过滤器= trackingKF属性:状态:x1双[4]StateCovariance: [4 x4双]MotionModel:“二维恒定速度”ProcessNoise: [2 x2双]MeasurementModel: [2 x4双]MeasurementNoise: [2 x2双]MaxNumOOSMSteps: 0 EnableSmoothing: 0

estimateStates (: 1) = filter.State;

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

通过递归地调用过滤器运行预测和正确的对象的功能。从结果,估计很快聚集在真相。实际上,线性卡尔曼滤波器具有指数收敛速度。

为我= 2:长度(tspan)预测(过滤器,dt) estimateStates正确(:,我)=(过滤器,测量(:,我));结束情节(estimateStates (1, 1), estimateStates (3,1),“g *”DisplayName =“初步估计”)情节(estimateStates (1:), estimateStates (3:)“g”DisplayName =“估计”传奇(位置=“东南”)

图包含一个坐标轴对象。坐标轴对象包含x (m), ylabel y (m)包含5类型的对象。一个或多个行显示的值只使用这些对象标记代表真正的初始,真理,测量,初步估计,估计。