机动目标跟踪
这个例子展示了如何使用各种跟踪滤波器来跟踪机动目标。这个例子展示了使用单一运动模型和多个运动模型的过滤器之间的区别。
定义一个场景
在这个例子中，您定义了一个目标，它最初以200米/秒的恒定速度移动33秒，然后进入一个10度/秒的恒定转弯。转弯持续33秒，然后目标以3m /s^2的速度直线加速。
[trueState, time, fig1] = helperGenerateTruthData;dt = diff(时间(1:2));numSteps =元素个数(时间);图(图一)
定义测量值为位置，并向测量值添加标准偏差为1的正常随机噪声。
%设置可重复结果的RNG种子s =提高;rng (2018);positionSelector = [1 0 0 0 0 0;0 0 0 0 0];%状态位置true = positionSelector * true;measNoise = randn(大小(truePos));measPos = truePos + measNoise;
跟踪使用恒定速度过滤器
你定义一个trackingEKF用等速运动模型。您使用第一个测量来定义初始状态和状态协方差，并将过程噪声设置为非加性的，以x、y和z分量中的未知加速度定义过程噪声。这个定义与函数类似initcvekf的工作原理。
initialState = positionSelector' * measPos(:，1);initialCovariance =诊断接头([1,1 e4 1 1 e4, 1, 1 e4]);没有测量速度cvekf = trackingEKF(@constvel， @cvmeas, initialState，．..“StateTransitionJacobianFcn”@constveljac,．..“MeasurementJacobianFcn”@cvmeasjac,．..“StateCovariance”initialCovariance,．..“HasAdditiveProcessNoise”假的,．..“ProcessNoise”、眼睛(3));
对于每个测量，您可以预测滤波器，计算预测状态与真实位置的距离，并使用测量对滤波器进行修正，以获得对位置的滤波估计。
dist = 0(1、numSteps);numSteps estPos = 0(3日);为i = 2:size(measPos,2) predict(cvekf, dt);dist (i) =距离(cvekf truePos(:,我));%离真实位置的距离estPos(:，i) = positionSelector * correct(cvekf, measPos(:，i));结束图(图一);情节(estPos (1:), estPos (2:)“.g”，“DisplayName的”，“简历PN低”)标题(“真实和估计的头寸”)轴([5000 8000 -500 2500])
如图所示，该滤波器能够很好地跟踪运动的匀速部分，但当目标执行转弯时，滤波器估计的位置与真实位置偏离。在下面的图中，你可以看到估计值与真实值的距离。在33-66秒的转弯过程中，归一化距离跳到非常高的值，这意味着滤波器无法跟踪机动目标。
图=图;持有在阴谋(1:numSteps) * dt,经销,‘g’，“DisplayName的”，“简历PN低”)标题(“从估计位置到真实位置的归一化距离”)包含(“时间(s)”) ylabel (“归一化距离”传说)
增加过程噪声
一个可能的解决方案是增加过程噪声。过程噪声表示运动模型中的未建模项。对于匀速模型，这些是未知的加速度项。通过增加过程噪声，可以在运动模型中考虑到更大的不确定性，这导致滤波器更多地依赖测量而不是模型。下面的线条创建一个恒定速度的过滤器，具有高的过程噪声值，对应大约5-G转弯。
cvekf2 = trackingEKF(@constvel， @cvmeas, initialState，．..“StateTransitionJacobianFcn”@constveljac,．..“MeasurementJacobianFcn”@cvmeasjac,．..“StateCovariance”initialCovariance,．..“HasAdditiveProcessNoise”假的,．..“ProcessNoise”诊断接头([50 50 1]));水平加速度的不确定性大dist = 0(1、numSteps);numSteps estPos = 0(3日);为i = 2:size(measPos,2) predict(cvekf2, dt);dist (i) =距离(cvekf2 truePos(:,我));%离真实位置的距离estPos(:，i) = positionSelector * correct(cvekf2, measPos(:，i));结束图(图一)情节(estPos (1:), estPos (2:)“c”，“DisplayName的”，“简历PN高”)轴([5000 8000 -500 2500])
增加过程噪声显著提高了滤波器在转弯过程中跟踪目标的能力。然而，这有一个代价:在运动的恒定速度周期内，滤波器不太能够平滑测量噪声。尽管在转弯过程中的归一化距离显著减少，但在匀速运动的前33秒，归一化距离增加了。
图(图)图((1:numSteps) * dt,经销,“c”，“DisplayName的”，“简历PN高”)轴([0 100 0 50])
使用交互运动模型过滤器
另一个解决方案是使用一个可以同时考虑所有运动模型的过滤器，称为交互多模型(IMM)过滤器。IMM过滤器可以维护尽可能多的运动模型，但通常用于2-5个运动模型。对于这个例子，三个模型是充分的:一个恒定速度模型，一个恒定转弯模型，和一个恒定加速度模型。
imm = trackingIMM (“TransitionProbabilities”, 0.99);%默认IMM有所有三种型号%根据第一个模型初始化状态和状态协方差初始化(imm initialState initialCovariance);
使用IMM滤波器的方式与使用EKF的方式相同。
dist = 0(1、numSteps);numSteps estPos = 0(3日);numSteps modelProbs = 0(3日);modelProbs (: 1) = imm.ModelProbabilities;为i = 2:size(measPos,2) predict(imm, dt);dist (i) =距离(imm, truePos(:,我));%离真实位置的距离estPos(:，i) = positionSelector * correct(imm, measPos(:，i));modelProbs (:, i) = imm.ModelProbabilities;结束图(图一)情节(estPos (1:), estPos (2:)“m”，“DisplayName的”，“IMM”）
的trackingIMM该滤波器能够跟踪机动目标在运动的三个部分。
检查滤波器的预测状态和真实位置之间的距离，你可以看到IMM滤波器能够减少运动的所有部分的距离。事实上，IMM滤波器在跟踪运动方面比之前使用的其他两种恒定速度模型都要好。
图(图二)在阴谋(1:numSteps) * dt,经销,“米”，“DisplayName的”，“IMM”)轴([0 100 0 50])
为了更好地理解IMM滤波器是如何工作的，请将模型概率作为时间的函数绘制出来。从图中可以看出，用三个具有相同概率的模型初始化滤波器。随着滤波器的更新，它很快收敛到一个非常高的概率，该模型是一个恒定速度模型。经过33秒的运动后，等速模型不再成立，在转弯的持续时间内，等速模型出现的概率变得非常高。在运动的最后一节中，在恒加速度机动过程中，IMM滤波器赋予运动为恒加速度的概率较高，但滤波器对正确的运动模型不太确定，运动为恒速度的概率约为0.3。
figure plot((1:numSteps)*dt, modelprobes) title(“模型概率vs.时间”)包含(“时间(s)”) ylabel (“模型概率”)传说(“IMM-CV”，“IMM-CA”，“IMM-CT”）返回RNG到之前的状态rng (s)
总结
这个例子向你展示了如何跟踪目标机动与恒定的转弯和恒定的加速度运动。该示例展示了如何通过增加过程噪声来捕获恒定速度模型的未知机动。您还看到了如何通过使用IMM滤波器来改进机动目标的跟踪。
金宝app支持功能
helperGenerateTruthData
这个函数生成地面真值轨迹。
函数[Xgt, tt, figs] = helperGenerateTruthData生成ground truthvx = 200;% m / sω= 10;%度/秒acc = 3;% m / s / sdt = 0.1;tt = (0: dt:地板(1000 * dt));无花果= [];元素个数Xgt =南(9日(tt));Xgt (: 1) = 0;%恒定的速度seg1 =地板(元素个数(tt) / 3);Xgt (2, 1) = vx;slct =眼(9);slct(三3:最终,)= [];为m = 2: s0 = slct*Xgt(:，m-1);X1 = convel (X0, dt);X1 = slct ' * X1;Xgt(:,米)= X1;结束%不变转seg2 =地板(2 *元素个数(tt) / 3);slct =眼(9);slct(三3:最终,)= [];为m = sg1 +1: sg2 X0 = slct*Xgt(:，m-1);X0 = (X0(1:4);ω);X1 = constturn(X0, dt);X1 = X1 (1:4);X1 = [X1 (1:2); 0; X1 (3:4); 0; 0 (3,1)];Xgt(:,米)= X1;结束%恒定加速度首先= true;为m = se2 +1:numel(tt) X0 = Xgt(:，m-1);如果first vel = X0(2:3:end);ua =韦尔/规范(或者);弗吉尼亚州= acc * ua;弗吉尼亚州X0(三3:结束)=;首先= false;结束X1 = constacc(X0, dt);Xgt(:,米)= X1;结束下落加速度尺寸slct =眼(9);slct(三3:最终,)= [];Xgt = slct * Xgt;Figs = [Figs figure];情节(Xgt (1,1: seg1), Xgt(3、1:seg1),“。”）;持有在；情节(Xgt (1, seg1 + 1: seg2), Xgt (3, seg1 + 1: seg2),“。”）;情节(Xgt (1, seg2 + 1:结束),Xgt (3, seg2 + 1:结束),“。”）;网格在；包含(“X位置(m)”）;ylabel (“Y位置(m)”）;标题(“正确的位置”)轴平等的；传奇(“恒定速度”，“恒转”，“恒定加速度”）结束
你也可以从以下列表中选择一个网站:
