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