扩展卡尔曼滤波器- MATLAB和Simulink金宝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`	恒定回转速率测量雅可比矩阵

示例:使用角度和距离测量来估计2-D目标状态`trackingEKF`

打开实时脚本

初始估计模型

假设一个目标以以下初始位置和速度在2D中移动。模拟持续20秒，采样时间为0.2秒。

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

假设测量值是相对于正x方向的方位角和从原点到目标的距离。测量噪声协方差矩阵为:

measureNoise = diag([2e-6;1]);测量噪声矩阵。单位是m²和rad²。

预分配保存结果的变量。

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

获得真实状态和测量值

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

为I = 2:长度(tspan)如果i ~= 1 trueStates(:，i) = stateModel(trueStates(:，i-1)，dt) + sqrt(processNoise)*randn(4,1);结束measurements(:，i) = measureModel(trueStates(:，i)) + 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)”)标题(“真正的轨迹”)轴广场

图中包含一个轴对象。标题为True Trajectory的axis对象包含2个类型为line的对象。这些物体代表初始真理，真实轨迹。

图(2)subplot(2,1,1) plot(tspan,measurements(1，:)*180/pi) xlabel(“时间(s)”) ylabel (“角(度))标题(“角度与范围”) subplot(2,1,2) plot(tspan,measurements(2，:)) xlabel(“时间(s)”) ylabel (“范围(m)”）

图中包含2个轴对象。带有标题Angle和Range的Axes对象1包含一个line类型的对象。坐标轴对象2包含一个line类型的对象。

初始化扩展卡尔曼滤波器

用初始状态估计值初始化过滤器[35;0;45;0]．

filter = trackingEKF(State=[35;0;45;0], StateCovariance = initialCovariance,.．.StateTransitionFcn = @stateModel ProcessNoise = ProcessNoise,.．.MeasurementFcn = @measureModel MeasurementNoise = measureNoise);estimateStates(:，1) = filter.State;

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

方法来运行筛选器预测而且正确的对象的功能。

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

图中包含一个轴对象。标题为True Trajectory vs Estimated Trajectory的axis对象包含4个类型为line的对象。这些对象代表初始值、真实轨迹、初始估计、估计轨迹。

辅助函数

stateModel模拟了无过程噪声的匀速运动。

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

meausreModel模型范围和方位角测量无噪声。

函数z = measureModel(state) angle = atan(state(3)/state(1));Range = norm([state(1) state(3)]);Z =[角度;范围];结束

另请参阅

trackingKF|trackingEKF|trackingUKF|线性卡尔曼滤波器