状态估计与包装测量使用扩展卡尔曼滤波gydF4y2Ba

这个示例使用:gydF4y2Ba

金宝app 金宝app

这个例子展示了如何使用扩展卡尔曼滤波算法对非线性状态估计三维跟踪涉及圆包角测量。目标跟踪,传感器通常采用球面坐标系的位置报告对象的方位,范围和海拔。从这组角测量报告在一定范围内。例如,报告方位角度的范围内gydF4y2Ba $- - - - - -gydF4y2Ba 1gydF4y2Ba 8gydF4y2Ba {0gydF4y2Ba}^{\circgydF4y2Ba}$ 来gydF4y2Ba $1gydF4y2Ba 8gydF4y2Ba {0gydF4y2Ba}^{\circgydF4y2Ba}$ 或gydF4y2Ba ${0gydF4y2Ba}^{\circgydF4y2Ba}$ 来gydF4y2Ba $3gydF4y2Ba 6gydF4y2Ba {0gydF4y2Ba}^{\circgydF4y2Ba}$ 。然而,跟踪通常发生在一个矩形框架,需要使用非线性滤波器可以处理所需的非线性转换球面测量转换成矩形。gydF4y2Ba

这个例子使用一个恒定的速度模型跟踪目标的三维位置和速度。扩展卡尔曼滤波器作为非线性滤波器跟踪物体的状态。这个过滤器使用方位(gydF4y2Ba $ψgydF4y2Ba$ )、范围和高程(gydF4y2Ba $ϕgydF4y2Ba$ )读数测量在此跟踪运动。gydF4y2Ba

source_diagram - 01. - pnggydF4y2Ba

这个示例的恒定速度三维植物模型由三个轴的位置和速度如下:gydF4y2Ba

$年代gydF4y2Ba tgydF4y2Ba 一个gydF4y2Ba tgydF4y2Ba egydF4y2Ba 年代gydF4y2Ba =gydF4y2Ba (gydF4y2Ba \begin{array}{c} xgydF4y2Ba \\ {xgydF4y2Ba}_{}^{˙gydF4y2Ba} \\ ygydF4y2Ba \\ {ygydF4y2Ba}_{}^{˙gydF4y2Ba} \\ zgydF4y2Ba \\ {zgydF4y2Ba}_{}^{˙gydF4y2Ba} \end{array}]gydF4y2Ba =gydF4y2Ba (gydF4y2Ba \begin{array}{c} xgydF4y2Ba \\ {vgydF4y2Ba}_{xgydF4y2Ba} \\ ygydF4y2Ba \\ {vgydF4y2Ba}_{ygydF4y2Ba} \\ zgydF4y2Ba \\ {vgydF4y2Ba}_{zgydF4y2Ba} \end{array}]gydF4y2Ba$

范围、方位角和高度角是由:gydF4y2Ba

$\begin{array}{l} RgydF4y2Ba 一个gydF4y2Ba ngydF4y2Ba ggydF4y2Ba egydF4y2Ba =gydF4y2Ba \sqrt{{xgydF4y2Ba}^{2gydF4y2Ba} +gydF4y2Ba {ygydF4y2Ba}^{2gydF4y2Ba} +gydF4y2Ba {zgydF4y2Ba}^{2gydF4y2Ba}} \\ EgydF4y2Ba lgydF4y2Ba egydF4y2Ba vgydF4y2Ba 一个gydF4y2Ba tgydF4y2Ba 我gydF4y2Ba ogydF4y2Ba ngydF4y2Ba (gydF4y2Ba ϕgydF4y2Ba)gydF4y2Ba =gydF4y2Ba {罪gydF4y2Ba}^{- - - - - -gydF4y2Ba 1gydF4y2Ba} (gydF4y2Ba \frac{zgydF4y2Ba}{RgydF4y2Ba 一个gydF4y2Ba ngydF4y2Ba ggydF4y2Ba egydF4y2Ba})gydF4y2Ba \\ 一个gydF4y2Ba zgydF4y2Ba 我gydF4y2Ba 米gydF4y2Ba ugydF4y2Ba tgydF4y2Ba hgydF4y2Ba (gydF4y2Ba ψgydF4y2Ba)gydF4y2Ba =gydF4y2Ba {棕褐色gydF4y2Ba}^{- - - - - -gydF4y2Ba 1gydF4y2Ba} (gydF4y2Ba \frac{ygydF4y2Ba}{xgydF4y2Ba})gydF4y2Ba \end{array}$

计算实际测量和预期之间的残余在过滤过程中是一个重要的步骤。没有必要的预防措施,对象的剩余价值可能是大包装边界附近。这导致滤波器发散的准确状态,导致一个不准确的状态估计。这个案例展示了如何通过使用测量包装处理这样的情况。仿真金宝app软件模型用于演示跟踪状态的差异在使用,没有测量包装。gydF4y2Ba

金宝appsimulink_diagram_ekf.pnggydF4y2Ba

状态估计——没有测量包装gydF4y2Ba

首先提供初始状态的猜测和指定模式名。gydF4y2Ba

rng (0) initialStateGuess = (-100 0 0 0 0 0) ';modelName =gydF4y2Ba“modelEKFWrappedMeasurements”gydF4y2Ba;gydF4y2Ba

生成基于测量噪声协方差如下:gydF4y2Ba

dt = 3;tSpan = 0: dt: 100;yTrue = 0(长度(tSpan), 3);yMeas = 0(大小(yTrue));noiseCovariance =诊断接头(20 [0.5 - 0.5]);xCurrent = initialStateGuess;gydF4y2Ba为gydF4y2Bai = 1:元素个数(tSpan) xTrue = stateTransitionFcn (xCurrent);:yTrue(我)= (measurementFcn (xTrue)) ';yMeas(我:)= yTrue(我:)+(胆固醇(noiseCovariance) * randn (3,1)) ';xCurrent = xTrue;gydF4y2Ba结束gydF4y2Ba

模拟模型。gydF4y2Ba

= sim (modelName);xEstNoWrap = 0(元素个数(tSpan),长度(initialStateGuess));xEstWithWrap = 0(元素个数(tSpan),长度(initialStateGuess));gydF4y2Ba

运行过滤器和vizualize状态收敛在一个3 d的阴谋。gydF4y2Ba

图()plot3 (initialStateGuess (1) initialStateGuess (3), initialStateGuess (5),gydF4y2Ba“罗”gydF4y2Ba,gydF4y2Ba“MarkerFaceColor”gydF4y2Ba,gydF4y2Ba“r”gydF4y2Ba,gydF4y2Ba“MarkerSize”gydF4y2Ba9);网格举行gydF4y2Ba在gydF4y2Ba;gydF4y2Ba%绘图选项gydF4y2Ba包含(gydF4y2Ba“x”gydF4y2Ba);ylabel (gydF4y2Ba“y”gydF4y2Ba);zlabel (gydF4y2Ba“z”gydF4y2Ba);标题(gydF4y2Ba“没有测量包装”gydF4y2Ba);gydF4y2Ba%运行过滤器gydF4y2Ba为gydF4y2Bai = 1:元素个数(tSpan) x = out.noWrapOut(我:);plot3 (x (1) x (3), x (5),gydF4y2Ba“波”gydF4y2Ba,gydF4y2Ba“MarkerFaceColor”gydF4y2Ba,gydF4y2Ba“b”gydF4y2Ba);持有gydF4y2Ba在gydF4y2Ba;:xEstNoWrap(我)= x;gydF4y2Ba结束gydF4y2Ba持有gydF4y2Ba从gydF4y2Ba

图包含一个坐标轴对象。坐标轴对象与标题没有测量包装,包含x, y ylabel包含35行类型的对象。一个或多个行显示的值只使用标记gydF4y2Ba

状态估计与测量包装gydF4y2Ba

现在,使测量包装并运行过滤器。在3 d可视化状态收敛阴谋。gydF4y2Ba

图()gydF4y2Ba%运行过滤器gydF4y2Ba为gydF4y2Bai = 1:元素个数(tSpan) x = out.wrapOut(我:);plot3 (x (1) x (3), x (5),gydF4y2Ba“波”gydF4y2Ba,gydF4y2Ba“MarkerFaceColor”gydF4y2Ba,gydF4y2Ba“b”gydF4y2Ba);持有gydF4y2Ba在gydF4y2Ba;:xEstWithWrap(我)= x;gydF4y2Ba结束gydF4y2Baplot3 (initialStateGuess (1) initialStateGuess (3), initialStateGuess (5),gydF4y2Ba“罗”gydF4y2Ba,gydF4y2Ba“MarkerFaceColor”gydF4y2Ba,gydF4y2Ba“r”gydF4y2Ba,gydF4y2Ba“MarkerSize”gydF4y2Ba9);网格标题(gydF4y2Ba“与测量包装”gydF4y2Ba);持有gydF4y2Ba从gydF4y2Ba

图包含一个坐标轴对象。坐标轴对象与测量包装标题包含35行类型的对象。一个或多个行显示的值只使用标记gydF4y2Ba

从上面的图中,观察到三个轴的规模远小于3 d图没有测量包装。这表示,有更好的收敛测量包装时启用。gydF4y2Ba

验证gydF4y2Ba

比较这两种情况下的估计错误在相同的情节。gydF4y2Ba

图()次要情节(3、1、1)的阴谋(xEstNoWrap (: 1)——xTrue (1));持有gydF4y2Ba在gydF4y2Ba情节(xEstWithWrap (: 1)——xTrue (1));ylim(1200[-500])标题(gydF4y2Ba“e_x”gydF4y2Ba);持有gydF4y2Ba从gydF4y2Ba次要情节(1、2)情节(xEstNoWrap (:, 3)——xTrue (3));持有gydF4y2Ba在gydF4y2Ba情节(xEstWithWrap (:, 3)——xTrue (3));ylim (1000 [-2000])gydF4y2Ba从gydF4y2Ba标题(gydF4y2Ba“e_y”gydF4y2Ba);次要情节(3、1,3)情节(xEstNoWrap (:, 5)——xTrue (5));持有gydF4y2Ba在gydF4y2Ba情节(xEstWithWrap (: 5)——xTrue (5));ylim (200 [-200])gydF4y2Ba从gydF4y2Ba标题(gydF4y2Ba“e_z”gydF4y2Ba);sgtitle (gydF4y2Ba“估计误差的阴谋”gydF4y2Ba)传说(gydF4y2Ba没有包装的gydF4y2Ba,gydF4y2Ba“包装”gydF4y2Ba,gydF4y2Ba“位置”gydF4y2Ba,gydF4y2Ba“最佳”gydF4y2Ba)gydF4y2Ba

图包含3轴subplottext类型的对象和另一个对象。坐标轴对象1标题e indexOf x基线包含2线类型的对象。2标题e indexOf y坐标轴对象基线包含2线类型的对象。z轴对象与标题3 e indexOf基线包含2线类型的对象。这些对象代表没有包装,包装。gydF4y2Ba

观察到的错误比较大得多的所有三个轴情况下没有测量包装。gydF4y2Ba

总结gydF4y2Ba

这个例子展示了如何使用扩展卡尔曼滤波的非线性系统的状态估计,没有测量包装。包装启用的状态估计是观察到提供更精确的状态估计由3 d和错误情节。gydF4y2Ba

金宝app支持功能gydF4y2Ba

状态转移函数gydF4y2Ba

扩展卡尔曼滤波块需要一个函数来描述状态的演变从一个时间步。这通常称为状态转换函数。对于这个示例,状态转换函数中包含的内容gydF4y2BastateTransitionFcn.mgydF4y2Ba如下:gydF4y2Ba

函数gydF4y2Bax = stateTransitionFcn dt (x) = 3;一个= [1 dt 0 0 0 0;0 1 0 0 0 0;0 0 1 dt 0 0;0 0 0 1 0 0;0 0 0 0 1 dt;0 0 0 0 0 1);x = x *;gydF4y2Ba结束gydF4y2Ba

测量功能gydF4y2Ba

扩展卡尔曼滤波块也需要一个函数,它描述了相关的模型状态传感器测量。这是测量功能。对于本例,测量函数中包含的内容gydF4y2BameasurementFcn.mgydF4y2Ba如下:gydF4y2Ba

函数gydF4y2Ba[y,边界]= measurementFcn (x) xPos = x (1);yPos = x (3);zPos = x (5);gydF4y2Ba%的范围内gydF4y2Bar =√xPos ^ 2 + yPos ^ 2 + zPos ^ 2);gydF4y2Ba%方位gydF4y2Baψ= atan2d (yPos xPos);gydF4y2Ba%海拔gydF4y2Baφ= asind (zPos / r);gydF4y2Ba%结合测量gydF4y2Bay = [r;常设调查小组委员会;φ);边界=[负无穷到正无穷;-180 180;-90 90);gydF4y2Ba结束gydF4y2Ba

另请参阅gydF4y2Ba

扩展卡尔曼滤波器gydF4y2Ba