扩展卡尔曼滤波器,MATLAB仿真软件金宝app - 金宝app,下载188bet金宝搏,金宝搏官方网站

扩展卡尔曼滤波器

当你使用一个过滤器来跟踪对象,使用一系列的检测或测量来估计状态一个对象的基于对象的运动模型。在运动模型中,国家的数量的集合代表一个对象的状态,比如它的位置,速度,加速度。使用一个扩展卡尔曼滤波器(trackingEKF)当物体运动遵循非线性状态方程或当测量非线性函数的状态。例如,考虑使用一个扩展卡尔曼滤波的测量对象在球坐标表示,如方位、仰角、和范围,但美国的目标在笛卡尔坐标表示。

制定基于扩展卡尔曼滤波的状态方程和测量方程的线性化。线性化使您能够传播的状态和状态协方差是一个近似的线性格式,并要求雅克比的状态方程和测量方程。

请注意

如果你估计系统是线性的,您可以使用线性卡尔曼滤波器(trackingKF)或扩展卡尔曼滤波器(trackingEKF)来估计目标状态。如果您的系统是非线性的,你应该使用一个非线性滤波器,如扩展卡尔曼滤波器或无味卡尔曼滤波(trackingUKF)。

状态更新模型

假设一个封闭的表达式预测状态的函数之前的状态x_k、控制u_k、噪音w_k和时间t。

$x_{k + 1} = f (x_{k}, u_{k}, w_{k}, t)$

雅可比矩阵的预测状态对之前的状态是通过偏导数为:

$F^{(x)} = \frac{\partial f}{\partial x} 。$

雅可比矩阵的预测状态对噪音:

$F^{(w)} = \frac{\partial f}{\partial w} 。$

这些函数有更简单的形式,当噪声添加剂在更新方程:

$x_{k + 1} = f (x_{k}, u_{k}, t) + w_{k}$

在这种情况下,F^(w)是一个单位矩阵。

您可以指定国家使用的雅可比矩阵StateTransitionJacobianFcn财产的trackingEKF对象。如果不指定这个属性,对象使用数值差分计算雅克比,稍微不那么准确,会增加计算时间。

度量模型

扩展卡尔曼滤波的测量也可以是一个非线性函数的状态和测量噪声。

$z_{k} = h (x_{k}, v_{k}, t)$

测量的雅可比矩阵的状态是:

$H^{(x)} = \frac{\partial h}{\partial x} 。$

雅可比矩阵的测量对测量噪声:

$H^{(v)} = \frac{\partial h}{\partial v} 。$

这些功能有更简单的形式,当噪声添加剂在测量方程:

$z_{k} = h (x_{k}, t) + v_{k}$

在这种情况下,H^(v)是一个单位矩阵。

在trackingEKF,您可以指定测量使用的雅可比矩阵MeasurementJacobianFcn财产。如果不指定这个属性,对象使用数值差分计算雅克比,稍微不那么准确,会增加计算时间。

扩展卡尔曼滤波回路

扩展卡尔曼滤波回路的循环是几乎相同的线性卡尔曼滤波器除了:

过滤器使用精确的非线性状态更新和测量功能。
雅可比矩阵取代了状态转移矩阵。
雅可比矩阵测量取代了测量矩阵。

扩展的卡尔曼滤波方程

预定义的扩展卡尔曼滤波的功能

工具箱提供了预定义的状态更新和度量函数来使用trackingEKF。

运动模型	函数名	函数的目的	状态表示
恒定的速度	`constvel`	常速状态更新模型	一维,`[x, vx)` 二维,`[x, vx; y; v]` 3 d -`[x, vx; y; v; z; vz]` 在哪里 `x`,`y`,`z`代表的位置x- - - - - -,y- - - - - -,z分别的方向。 `vx`,`v`,`vz`代表着速度x- - - - - -,y- - - - - -,z分别的方向。
	`constveljac`	常速状态更新雅可比矩阵
	`cvmeas`	常速测量模型
	`cvmeasjac`	常速测量雅可比矩阵
恒定的加速度	`constacc`	加速度恒定状态更新模型	一维,`[x, vx; ax)` 二维,`[x, vx;斧子;y v,唉)` 3 d -`[x, vx;斧子;y v;是的;z; vz; az)` 在哪里 `斧头`,`唉`,`阿兹`代表的加速度x- - - - - -,y- - - - - -,z分别的方向。
	`constaccjac`	雅可比矩阵恒定加速度状态更新
	`cameas`	恒定加速度测量模型
	`cameasjac`	雅可比矩阵恒定加速度测量
不断的周转周期	`constturn`	不断的周转周期状态更新模型	二维,`[x, vx; y, v,ω)` 3 d -`[x, vx; y, v,ω;z; vz]` 在哪里`ω`代表了周转周期。
	`constturnjac`	不断的周转周期状态更新雅可比矩阵
	`ctmeas`	不断的周转周期测量模型
	`ctmeasjac`	不断的周转周期测量雅可比矩阵

例如:估计二维目标角度和距离测量使用`trackingEKF`

打开生活的脚本

初始化估计模型

假设一个目标在2 d用下面的初始位置和速度。仿真持续20秒采样时间为0.2秒。

rng (2022);%的可重复的结果dt = 0.2;%秒simTime = 20;%秒tspan = 0: dt: simTime;trueInitialState = [30;1;40;1);% (x, vx; y; v)initialCovariance =诊断接头([1 100、1 e3,100 e3]);processNoise =诊断接头([0;. 01;0;. 01]);%过程噪声矩阵

假设是方位角测量相对于正x方向和范围从原点到目标地点。测量噪声协方差矩阵:

measureNoise =诊断接头([2 e-6; 1]);%测量噪声矩阵。单位是m ^ 2和rad ^ 2。

Preallocate变量来保存结果。

numSteps =长度(tspan);numSteps trueStates =南(4日);trueStates (: 1) = trueInitialState;estimateStates =南(大小(trueStates));测量=南(2,numSteps);

获得真正的状态和测量

恒定的速度传播模型和产生噪声的测量。

为我= 2:长度(tspan)如果我~ = 1 trueStates (:, i) = stateModel (trueStates:张)(dt) + sqrt (processNoise) * randn (4,1);结束测量(:,i) = measureModel (trueStates(:,我))+ sqrt (measureNoise) * randn (2, 1);结束

真正的轨迹和测量的阴谋。

图(1)情节(trueStates (1, 1), trueStates (3,1),“r *”DisplayName =“最初的真理”)举行在情节(trueStates (1:), trueStates (3:)“r”DisplayName =“真正的轨迹”)包含(“x”(m))ylabel (“y (m)”)标题(“真正的轨迹”)轴广场

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

图(2)次要情节(2,1,1)情节(tspan,测量(1:)* 180 / pi)包含(“时间(s)”)ylabel (“角(度))标题(“角度和范围”次要情节(2,1,2)情节(tspan,测量(2:))包含(“时间(s)”)ylabel (“范围(m)”)

图包含2轴对象。坐标轴对象1标题角度和范围,包含时间(s), ylabel角(度)包含一个类型的对象。坐标轴对象2包含时间(s), ylabel范围(m)包含一个类型的对象。

初始化扩展卡尔曼滤波器

初始化滤波器的初始状态估计[35;0;45;0]。

过滤器= trackingEKF(状态= [35;0;45;0],StateCovariance = initialCovariance,…StateTransitionFcn = @stateModel ProcessNoise = ProcessNoise,…MeasurementFcn = @measureModel MeasurementNoise = measureNoise);estimateStates (: 1) = filter.State;

运行扩展卡尔曼滤波器,并显示结果

通过递归地调用过滤器运行预测和正确的对象的功能。

为我= 2:长度(tspan)预测(过滤器,dt);estimateStates (:, i) =正确(过滤器,测量(:,我));结束图(1)情节(estimateStates (1, 1), estimateStates (3,1),“g *”DisplayName =“初步估计”)情节(estimateStates (1:), estimateStates (3:)“g”DisplayName =“估计轨迹”传奇(位置=“西北”)标题(“真正的轨迹与估计轨迹”)

图包含一个坐标轴对象。坐标轴对象与标题真正轨迹vs估计轨迹,包含x (m), ylabel y (m)包含4线类型的对象。一个或多个行显示的值只使用这些对象标记代表最初的真理,真正的轨迹,初步估计,估计轨迹。

辅助函数

stateModel建模没有过程噪声恒定速度运动。

函数stateNext = stateModel(状态,dt) F = [1 dt 0 0;0 1 0 0;0 0 1 dt;0 0 0 1);stateNext = F *状态;结束

meausreModel没有噪声模型和方位角的测量范围。

函数z = measureModel(州)角=:状态(3)/ (1));范围=规范([状态(3)(1)]);z =(角;范围);结束

另请参阅

trackingKF|trackingEKF|trackingUKF|线性卡尔曼滤波器|介绍估计过滤器