这个例子展示了如何使用各种跟踪滤波器来跟踪机动目标。这个例子展示了使用单一运动模型和多个运动模型的过滤器之间的区别。
在此示例中,您定义一个单个目标,其最初在恒定速度下行进200 m / s 33秒,然后进入恒定的转弯10 deg / s。转弯持续33秒,然后目标在3 m / s ^ 2的直线上加速。
[Truestate,Time,Fig1] = HelperGeneratetrethdata;dt = diff(时间(1:2));numsteps = numel(时间);图(图)
将测量值定义为位置并添加正常随机噪声,标准偏差为1到测量。
%设置可重复结果的RNG种子S = RNG;RNG(2018);PositionSelector = [1 0 0 0 0 0; 0 0 1 0 0 0; 0 0 0 0 1 0];%状态位置true = positionSelector * true;measNoise = randn(大小(truePos));measPos = truePos + measNoise;
你定义A.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',初始转动性,......“HasAdditiveProcessNoise”, 错误的,......'processnoise',眼睛(3));
对于每次测量,您预测过滤器,从真实位置计算预测状态的距离,并使用测量校正过滤器以获得滤波的估计位置。
dist = 0(1、numSteps);numSteps estPos = 0(3日);为了i = 2:尺寸(measpos,2)预测(cvekf,dt);DIST(i)=距离(CVEKF,TRUEPOS(:,i));从真实位置距离estPos(:,i) = positionSelector * correct(cvekf, measPos(:,i));结尾图(图一);情节(estPos (1:), estPos (2:)“.g”那'显示名称'那'cv低pn')标题(“真实和估计的头寸”)轴([5000 8000 -500 2500])
如图所示,该滤波器能够很好地跟踪运动的匀速部分,但当目标执行转弯时,滤波器估计的位置与真实位置偏离。在下面的图中,你可以看到估计值与真实值的距离。在33-66秒的转弯过程中,归一化距离跳到非常高的值,这意味着滤波器无法跟踪机动目标。
图=图;持有在绘图((1:numsteps)* dt,dist,‘g’那'显示名称'那'cv低pn')标题(“从估计位置到真实位置的归一化距离”)包含(“时间(s)”) ylabel (“归一化距离”) 传奇
一个可能的解决方案是增加过程噪声。过程噪声表示运动模型中的未建模项。对于匀速模型,这些是未知的加速度项。通过增加过程噪声,可以在运动模型中考虑到更大的不确定性,这导致滤波器更多地依赖测量而不是模型。下面的线条创建一个恒定速度的过滤器,具有高的过程噪声值,对应大约5-G转弯。
cvekf2 = trackingekf(@constvel,@cvmeas,initialstate,......'stateTransitionJacobianfcn',@constveljac,......'measurementjacobianfcn'@cvmeasjac,......'StateCovariance',初始转动性,......“HasAdditiveProcessNoise”, 错误的,......'processnoise',诊断([50,50,1]));水平加速度的不确定性大dist = 0(1、numSteps);numSteps estPos = 0(3日);为了i = 2:尺寸(meSPOS,2)预测(CVEKF2,DT);DIST(i)=距离(CVEKF2,TRUEPOS(:,i));从真实位置距离estPos(:,i) = positionSelector * correct(cvekf2, measPos(:,i));结尾图(图一)情节(estPos (1:), estPos (2:)“c”那'显示名称'那“简历PN高”)轴([5000 8000 -500 2500])
增加过程噪声显着提高了滤波器在转弯期间跟踪目标的能力。然而,有一种成本:过滤器在运动的恒定速度期间能够平滑测量噪声。即使转弯期间的归一化距离显着降低,在运动的恒定速度期间,在前33秒内归一化距离也增加。
图(图2)绘图((1:numsteps)* dt,dist,'C'那'显示名称'那“简历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)= positeSelector *正确(Imm,Measpos(:,i));ModelProbs(:,i)= imm.modelprobabilities;结尾图(图一)情节(estPos (1:), estPos (2:)'.m'那'显示名称'那'IMM'的)
这Trackingimm.
过滤器能够在运动的所有三个部分中跟踪机动目标。
检查滤波器的预测状态和真实位置之间的距离,你可以看到IMM滤波器能够减少运动的所有部分的距离。事实上,IMM滤波器在跟踪运动方面比之前使用的其他两种恒定速度模型都要好。
图(图2)保持在绘图((1:numsteps)* dt,dist,“米”那'显示名称'那'IMM')轴([0 100 0 50])
为了更好地理解IMM滤波器是如何工作的,请将模型概率作为时间的函数绘制出来。从图中可以看出,用三个具有相同概率的模型初始化滤波器。随着滤波器的更新,它很快收敛到一个非常高的概率,该模型是一个恒定速度模型。经过33秒的运动后,等速模型不再成立,在转弯的持续时间内,等速模型出现的概率变得非常高。在运动的最后一节中,在恒加速度机动过程中,IMM滤波器赋予运动为恒加速度的概率较高,但滤波器对正确的运动模型不太确定,运动为恒速度的概率约为0.3。
图绘图((1:numsteps)* dt,modelprobs)标题('模型概率与时间')包含(“时间(s)”) ylabel (“模型概率”) 传奇(“IMM-CV”那“IMM-CA”那“IMM-CT”的)返回RNG到之前的状态rng(s)
这个例子向你展示了如何跟踪目标机动与恒定的转弯和恒定的加速度运动。该示例展示了如何通过增加过程噪声来捕获恒定速度模型的未知机动。您还看到了如何通过使用IMM滤波器来改进机动目标的跟踪。
Helpergeneratetrethdata.
此函数生成地面真理轨迹。
函数[Xgt, tt, figs] = helperGenerateTruthData%产生地面真理Vx = 200;% 多发性硬化症欧米茄= 10;%度/秒acc = 3;%m / s / sdt = 0.1;TT =(0:DT:地板(1000 * DT));图= [];XGT = NaN(9,NUMER(TT));XGT(:,1)= 0;%恒定速度SEG1 =地板(NUMER(TT)/ 3);XGT(2,1)= Vx;slct =眼(9);SLCT(3:3:结束,:) = [];为了m = 2:seg1 x0 = slct * xgt(:,m-1);x1 = constvel(x0,dt);x1 = slct'* x1;XGT(:,m)= x1;结尾%不断转弯seg2 =地板(2 *元素个数(tt) / 3);slct =眼(9);SLCT(3:3:结束,:) = [];为了m = seg1 + 1:seg2 x0 = slct * xgt(:,m-1);x0 = [x0(1:4); omega];x1 = constrturn(x0,dt);x1 = x1(1:4);x1 = [x1(1:2); 0; x1(3:4); 0;零(3,1)];XGT(:,m)= 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(:,m)= 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:结束),'.-');网格在;Xlabel(“X位置(m)”);ylabel('y位置(m)');标题(“正确的位置”)轴平等的;传奇('恒速'那'不断转弯'那'不断加速'的)结尾