使用滤波、平滑和插值从采样数据重建地面真实轨迹
这个例子展示了如何使用一个基于交互式运动模型的滤波器以及一个平滑器来重建基于插值技术的地面真实轨迹。
使用跟踪过滤器
许多轨迹可以被分割成一系列离散的机动，其中每个机动都由一小组参数控制，这些参数控制物体的姿态如何随时间变化。这些机动通常由非常简单的模型组成，例如，恒定速度、恒定转弯或恒定加速度模型。这些简单的模型可以与跟踪滤波器一起使用，它可以容忍参数的小扰动。
虽然这些扰动应该很小，但它们不一定是随机的。例如，恒定速度滤波器通常可以跟踪一个具有小恒定加速度的对象，而代价是跟踪结果中的小偏差。当这些扰动很明显时，滤波器的性能会严重下降。
在机动目标跟踪例如，您将学习如何使用交互多模型(IMM)滤波器跟踪目标，其轨迹可以分为三个部分:单个恒定速度，恒定转弯和恒定加速度部分。IMM筛选器是一个“混合”筛选器，它并行运行一组筛选器，并返回一个结果，该结果根据每个筛选器的性能对估计值进行加权。你可以看到下面这个例子的结果:
N = 1000;[trueState, time, fig1] = helperGenerateTruthLoop(n);Dt = diff(时间(1:2));numSteps = numel(时间);rng (2021);%以获得可重复的结果%从9维状态中选择位置[x;vx;ax;y;vy;ay;z;vz;az]positionSelector = [1 0 0 0 0 0 0 0 0 0 0;0 0 0 1 0 0 0 0;0 0 0 0 0 0 1 0 0];truePos = positionSelector*trueState;Sigma = 10;%测量噪声标准差measNoise = sigma* randn(size(truePos));measPos = truePos + measNoise;initialState = positionSelector'*measPos(:，1);initialCovariance = diag([1 1e4 1e4 1 1e4 1e4 1 1e4 1e4]);没有测量速度和加速度检测= objectDetection(0，[0;0;0),“MeasurementNoise”，sigma^2 * eye(3));F1 = initcaekf(检测);%恒定加速度EKFf1。ProcessNoise = 0.3*eye(3);F2 = initctekf(检测);%恒定转弯EKFf2。ProcessNoise = diag([1 1 100 1]);F3 = initcvekf(检测);%等速EKFimm = trackingIMM({f1;f2;f3},“TransitionProbabilities”, 0.99,.．.“ModelConversionFcn”, @switchimm);初始化(imm, initialState, initialCovariance);estState = 0 (9,numSteps);为i = 2:size(measPos,2) predict(imm,dt);estState(:，i) =正确(imm,measPos(:，i));结束图(图一);Ax = gca(图1);线(ax, estState (1:), estState (4:), estState (7:)“颜色”，“k”，“线型”，“:”，“DisplayName的”，校正估计的）;轴(ax,“图像”）;
放大每个模型之间的过渡区域:
轴(ax，[6000 7000 -200 200])“位置”，“bestoutside”）
IMM滤波器面临着运动模型从一个到另一个变化后的时间间隔的挑战。在每一个过渡区域，向心加速度在恒速度和恒加速度模型中不被考虑。因此，估计偏差持续一小段时间，直到IMM滤波器有足够的数据来确定正确的运动模型。此后，偏差消失，直到下一个过渡区域。
平滑轨迹估计
平滑是一种有效的改进轨迹估计的方法。平滑是通过应用向前和向后滤波器来实现的。平滑器可以选择向前和向后预测区域中的“最佳”，以减轻两次机动之间过渡区域的预测误差。您使用与上面相同的数据和过滤器设置，但是启用了平滑功能。
imm = trackingIMM({f1;f2;f3},“TransitionProbabilities”, 0.99,.．.“ModelConversionFcn”@switchimm,.．.“EnableSmoothing”,真的,.．.“MaxNumSmoothingSteps”大小(measPos 2) 1);初始化(imm, initialState, initialCovariance);estState = 0 (9,numSteps);为i = 2:size(measPos,2) predict(imm,dt);estState(:，i) =正确(imm,measPos(:，i));结束smoothState =平滑(imm);线(ax, smoothState (1:), smoothState (4:), smoothState (7:)“颜色”，‘g’，“线型”，“- - -”，“DisplayName的”，“平滑估计”)轴(ax,“图像”）;
放大与上面相同的区域，你可以看到平滑估计在恢复地面真相轨迹方面要成功得多。
轴(ax，[6000 7000 -200 200])
从结果中可以看出，在从一个运动模型过渡到另一个具有突变的运动模型时，平滑是恢复轨迹数据的有效工具。这种前后平滑通常可以很好地估计地面真相轨迹。
密集采样数据的低阶多项式
在比较跟踪算法时，将跟踪结果表示为一种格式，以便可以查询任意时间瞬间的姿态信息是有利的。当数据以足够高的速率采样时，您可以使用一组低阶分段多项式来近似数据。这些多项式可以微分得到速度和加速度信息。此外，您可以使用一组算法来基于某些假设推断对象的方向。例如，假设一个物体总是面向运动方向，倾斜以抵消向心力。下面的代码展示了如何使用平滑的位置和速度，通过使用三次样条来推断物体的总加速度。
smoothPos = smoothState([1 4 7]，:);smoothVel = smoothState([2 5 8]，:);平滑状态向量不包括加速度从速度信息创建分段多项式velPP = pchip(time(2:end)，smoothVel);构造一个新的带导数的分段多项式[休息,系数,~,k,昏暗的]= unmkpp (velPP);Coefs = Coefs (:，1:k-1).*(k-1:-1:1);accPP = mkpp(休息、系数、暗);计算分段多项式得到加速度smoothAcc = ppval(accPP,time(2:end));显示速度和产生的加速度图;Ax1 = subplot(2,1,1，“父”图);情节(ax₁,时间(2:结束),smoothVel(: 1)、时间(2:结束),smoothVel(2:)、时间(2:结束),smoothVel (3:));包含(“时间[s]”）;ylabel (“速度(米/秒)”)标题(“速度剖面”)传说(“v_x”，“v_y”，“v_z”) ax2 = subplot(2,1,2，“父”图);情节(ax2、时间(2:结束),smoothAcc(: 1)、时间(2:结束),smoothAcc(2:)、时间(2:结束),smoothAcc (3:));包含(“时间[s]”) ylabel (“加速度(m / s ^ 2)”)标题(“加速概要”)传说(“a_x”，“得+”，“a_z”）
在恒定转弯机动中，计算的加速度不是纯正弦的，这是由于平滑输出中的小变化。然而，结果提供了信息，以确定每个机动的不同区域。
虽然三次多项式在数值上是有效的插值位置，但它们依赖于密集采样来忠实地表示圆弧。如果需要更高的准确性，则需要以更高的速率使用采样数据，这需要增加内存资源和更大的查找表。
使用高阶多项式
在前一节中，您使用三次多项式来计算每个点的加速度。对于主要由圆形和直线运动组成的轨迹，使用有利于恒定曲率的插值比低阶多项式工作得更好。的waypointTrajectory对象可以用恒定的转弯(恒定的曲率)建模轨迹。它还可以忠实地表示曲率和加速度都逐渐变化的轨迹。
减少插补内存占用和减少瞬态大小的一个简单技术是使用变点估计来定位轨迹曲率的显著变化。然后，用这些点来计算插值。
得到投影到x-y平面上的轨迹曲率planar曲率=交叉(smoothVel,smoothAcc) ./ vecnorm(smoothVel,2).^3;horizontalCurvature = planarCurvature(3，:);获得变更点的估计值阈值= var(horizontal曲率);[tf,m] = ischange(horizontal曲率，“阈值”阈值);无花果=图;甘氨胆酸hAx =(图);线(hAx、时间(2:结束),horizontalCurvature,“DisplayName的”，“计算”）;[xx, yy] =楼梯(时间(2:结束),m);线(hAx, xx和yy,“DisplayName的”，“估计”，“颜色”，“k”) xlabel(“时间[s]”）;ylabel (的曲率(rad / m))标题(横向曲率的）
然后你可以把这些变化点作为构造轨迹的路径点的起点:
Idx = 1+[0 find(tf) number (tf)-1];waypoints = smoothPos(:，idx)';wayvels = smoothVel(:，idx)';timeOfArrival =时间(idx);traj =路径点轨迹(路径点，到达时间，“速度”, wayvels);[wayPos， ~， wayVel] = lookupPose(traj,time(1:end-1));clf情节(trueState (1:), trueState (4:)“- - -”）;持有在；情节(wayPos (: 1) wayPos (:, 2),“- - -”)情节(锚点(:1)锚点(:,2),“o”）;网格在；包含(X位置[m]）;ylabel (Y位置[m]）;轴平等的；传奇(“真相”，“航点估计”，“锚点”，“位置”，“西南”）
从结果中，您仅使用几个路径点和它们对应的速度就恢复了圆形轨迹。然而，半径对过渡区域内估计速度的切线角很敏感——这对于跟踪滤波器来说是一个很难预测的区域。
如果将重建轨迹的位置误差大小与平滑估计绘制出来，可以看到误差较大的局部区域:
图;情节(时间(2:结束),vecnorm (smoothPos-wayPos ', 2)包含(“时间[s]”) ylabel (“距离[m]”)标题(“位置误差幅度”）;
请注意，在路径点与平滑对应点接近的区域中，路径点非常接近，而在远离路径点的区域中，路径点的偏差较大。减少误差的一个简单方法是在误差最大的位置反复添加额外的路点，直到达到预期的目标。在这个例子中，如果你为最大误差设置了10米的目标，你只需要更多的路径点。
目标= 10;%使用10米为目标最大位置误差maxError = Inf;将错误预分配为最大值而maxError > objective [~，maxi] = max(vecnorm(smoothPos-wayPos'，2));Idx = unique(sort([maxi Idx]));waypoints = smoothPos(:，idx)';wayvels = smoothVel(:，idx)';timeOfArrival =时间(idx);traj =路径点轨迹(路径点，到达时间，“速度”, wayvels);[wayPos， ~， wayVel, wayAcc] = lookupPose(traj,time(1:end-1));maxError = max(vecnorm(smoothPos-wayPos'，2));结束图;情节(trueState (1:), trueState (4:)“- - -”）;持有在；情节(wayPos (: 1) wayPos (:, 2),“- - -”)情节(锚点(:1)锚点(:,2),“o”）;网格在；包含(X位置[m]）;ylabel (Y位置[m]）;轴平等的；传奇(“真相”，“航点估计”，“锚点”，“位置”，“西南”）
然后你可以观察到位置误差的减小。
图;情节(时间(2:结束),vecnorm (smoothPos-wayPos ', 2)包含(“时间[s]”) ylabel (“距离[m]”)标题(“位置误差幅度”）;
现在你已经有了一个合理的位置轮廓，你可以检查从弹道得到的速度和加速度轮廓。注意在正弦加速度曲线恢复方面的整体改进。
图;Ax1 = subplot(2,1,1，“父”图);情节(ax₁,时间(1:end-1) wayVel);标题(“速度剖面”)包含(“时间”）;ylabel (“速度(米/秒)”)传说(“v_x”，“v_y”，“v_z”) ax2 = subplot(2,1,2，“父”图);情节(ax2、时间(1:end-1) wayAcc);标题(“加速概要”)包含(“时间[s]”）;ylabel (“加速度(m / s ^ 2)”)传说(“a_x”，“得+”，“a_z”）
总结
在本例中，您学习了如何使用交互的多模型平滑器和低阶多项式重建地面真相轨迹。您还使用了一个简单的方案，该方案使用了高阶插值器(包含在waypointTrajectory对象)以及关键采样，以恢复高阶导数，否则将被噪声掩盖。
您也可以从以下列表中选择一个网站: