机动目标跟踪
这个例子展示了如何使用各种跟踪机动目标跟踪滤波器。过滤器的示例显示了区别使用单个运动模型和多个运动模型。
定义一个场景
在本例中,您定义一个目标,最初以一个恒定的速度200米/秒的33秒,然后进入一个常数的10度/秒。将持续33秒,然后目标在直线加速3 m / s ^ 2。
(trueState、时间图一)= helperGenerateTruthData;dt = diff(时间(1:2));numSteps =元素个数(时间);图(图一)
定义测量的位置和正常添加随机噪声测量的标准偏差为1。
%设置RNG种子可重复的结果s =提高;rng (2018);positionSelector = [1 0 0 0 0 0; 0 0 1 0 0 0, 0 0 0 0 0 1);%的位置从国家truePos = positionSelector * trueState;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日);为我= 2:尺寸(measPos, 2)预测(cvekf dt);dist (i) =距离(cvekf truePos(:,我));%距离真正的位置estPos (:, i) = positionSelector *正确(cvekf, measPos(:,我));结束图(图一);情节(estPos (1:), estPos (2:)“.g”,“DisplayName的”,“简历PN低”)标题(“真的,估计位置”轴((5000 8000 -500 2500))
描绘在图中,滤波器可以跟踪恒定速度运动的一部分,但当目标执行,滤波器估计位置偏离真正的位置。你可以看到从真理的距离估计在接下来的情节。期间,在33 - 66秒,归一化距离跳跃非常高的值,这意味着过滤器无法跟踪机动目标。
图=图;持有在阴谋(1:numSteps) * dt,经销,‘g’,“DisplayName的”,“简历PN低”)标题(“归一化距离估计位置为True位置”)包含(“时间(s)”)ylabel (“归一化距离”传说)
增加过程噪声
一个可能的解决方案是增加过程噪声。过程噪声代表了未建模方面的运动模型。为一个恒定的速度模型,这些都是未知的加速度。通过增加过程噪音,你允许更大的运动模型的不确定性,导致过滤器在测量多依赖模型。以下行创建一个恒定的速度过滤、处理噪音值对应于高约5克。
cvekf2 = trackingEKF (@constvel @cvmeas initialState,…“StateTransitionJacobianFcn”@constveljac,…“MeasurementJacobianFcn”@cvmeasjac,…“StateCovariance”initialCovariance,…“HasAdditiveProcessNoise”假的,…“ProcessNoise”诊断接头([50 50 1]));%大不确定性水平加速度dist = 0 (1、numSteps);numSteps estPos = 0(3日);为我= 2:尺寸(measPos, 2)预测(cvekf2 dt);dist (i) =距离(cvekf2 truePos(:,我));%距离真正的位置estPos (:, i) = positionSelector *正确(cvekf2, measPos(:,我));结束图(图一)情节(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滤波器,卡尔曼滤波器使用。
dist = 0 (1、numSteps);numSteps estPos = 0(3日);numSteps modelProbs = 0(3日);modelProbs (: 1) = imm.ModelProbabilities;为我= 2:尺寸(measPos, 2)预测(imm) dt);dist (i) =距离(imm, truePos(:,我));%距离真正的位置estPos (:, i) = positionSelector *正确(imm, measPos(:,我));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的概率一个恒定的速度运动。
图绘制((1:numSteps) * dt, modelProbs)标题(“模型概率与时间”)包含(“时间(s)”)ylabel (“模型概率”)传说(“IMM-CV”,“IMM-CA”,“IMM-CT”)% RNG回到之前的状态rng (s)
总结
这个例子向您展示了如何跟踪目标机动和常数,常数加速度运动。示例显示如何增加过程噪声以一个恒定的速度获取未知的机动模型。您还了解了如何提高机动目标的跟踪,利用IMM滤波器。
万博1manbetx支持功能
helperGenerateTruthData
这个函数生成地面实况轨迹。
函数= helperGenerateTruthData Xgt, tt,无花果%生成地面实况vx = 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: seg1 X0 = slct * Xgt (:, m - 1);X1 = constvel (X0, dt);X1 = slct ' * X1;Xgt(:,米)= X1;结束%不变转seg2 =地板(2 *元素个数(tt) / 3);slct =眼(9);slct(三3:最终,)= [];为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(:,米)= X1;结束%恒定加速度首先= true;为m = seg2 + 1:元素个数(tt) X0 = Xgt (:, m - 1);如果第一个或者= X0(2:3:结束);ua =韦尔/规范(或者);弗吉尼亚州= acc * ua;弗吉尼亚州X0(三3:结束)=;首先= false;结束X1 = constacc (X0, dt);Xgt(:,米)= X1;结束%滴加速度维度slct =眼(9);slct(三3:最终,)= [];Xgt = slct * Xgt;无花果=(无花果图);情节(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)”);标题(“正确的位置”)轴平等的;传奇(“恒定速度”,“恒转”,“恒定加速度”)结束
