的平滑轨迹估计trackingIMM筛选
方法平滑目标的状态估计光滑的对象功能。使用最新的测量和状态估计信息，平滑是一种优化先前状态估计的技术。在此示例中，您将学习如何通过运行后向递归来了解从交互多模型（IMM）滤波器的先前校正的估计，这会产生平滑和更准确的状态估计。在第一部分中，实现一个平滑的算法，以平滑车削车的轨迹。在这个例子的其余部分中，您在几个高机动的飞机轨迹上进行平滑，从而从多目标跟踪的基准轨迹例子．
固定间隔平滑以跟踪车削车
在本节中，您可以平滑车削车的轨迹估计。首先，你使用helperGenerateCarMeasurements功能生成汽车的位置测量和相应的真理。
rng (2021);为可重复的结果％为滚动的汽车产生测量[measPositionCar, trueStateCar, timeCar] = helperGenerateCarMeasurements();%以objectDetection的格式定义初始检测检测= objectDetection(0， [0;0;0]，'MeasurementNoise'，眼睛（3,3））;
使用initekfimm初始化功能，基于恒定速度，恒定加速度和恒定转动运动模型创建IMM滤清器。要设置滤波器以便向后平滑，请设置EnableSmoothing过滤器的属性真正的．由于您希望在状态估计上执行固定间隔平滑（脱机平滑），因此使用所有更正和预测的步骤，您可以设置maxnumsmoothingsteps.属性设置为度量步骤的数量。
defaultIMMCar = initekfimm(检测);％能够平滑defaultimmcar.enablessmoothing = true;defaultimmcar.maxnumsmoothingsteps = size（measpositioncar，1）;％initilaize immpositionSelector = [1 0 0 0 0 0; 0 0 1 0 0 0, 0 0 0 0 0 1);从状态中选择位置initialstate = positionSelector'* mosePositionCar（1，:)';initialcovariance = diag（[1,1e4,1,1e4,1,1e4]）;初始化（Defaultimmcar，InitialState，InitialCovariance）;
要获得汽车的正向估计，可以通过迭代预测和修正状态估计来运行过滤器。
％初始化纠正状态numSteps =元素个数(timeCar);numSteps correctState = 0(6日);correctStateCovariance = 0 (6 6 numSteps);correcState (: 1) = defaultIMMCar.State;correctStateCovariance (:,: 1) = defaultIMMCar.StateCovariance;用预测和校正的％前进跟踪为了i = 2：numsteps dt = timecar（i） - timecar（i-1）;预测（DefaultImmcar，DT）;[校舍（：，i），纠正架尺寸（：，：，i）] =正确（DefecureImmcar，mosePositionCar（i，:)）;结尾
要执行平滑，只需拨打电话光滑的过滤器的对象函数。该函数返回平滑状态、状态协方差和模型概率。
[SmoothState，SmoothStatecovariance，ModelPababilities] =平滑（DefaultImmcar）;
接下来，使用HelpertrajectoryViewer.功能可视化平滑结果和RMS错误。结果表明，使用IMM滤波器的离线平滑使您可以减少估计中的错误。
trajnum = 0;HelpertrajectoryViewer（Treajnum，Timecar，Corricstate，Shimplystate，Truestatecar，MoSpositionCar）;
使用默认IMM配置平滑高机动飞机轨迹
在本节中，您可以平滑六架高机动飞机的轨迹。本节中使用的轨迹与其中相同多目标跟踪的基准轨迹的例子。在这个例子中，飞机的加速度变化高达35
在一些演习中。您使用helperGenerateAircraftTrajMeasurements函数生成测量数据和真理。
[measuposition, trueState, time] = helperGenerateAircraftTrajMeasurements;
配置类似于上一节的IMM滤清器。
%定义初始检测检测= objectDetection(0， [0;0;0]，'MeasurementNoise'，眼睛（3,3））;defaultimmaircraft = initekfimm（检测）;％能够平滑defaultimmaircraft.EnablesSmoothing = true;defaultimmaircraft.maxnumsmoothingsteps = size（measposition，1）;
使用helperGenerateSmoothData函数，运行创建的IMM滤波器，获取校正状态估计，并生成第一个轨迹的平滑状态估计。
只估计第一个轨迹trajNum = 1;提取测量数据measpostraj = meSbosition（：，：，trajnum）;使用HelperGeneratesSmoothData功能估计和平滑轨迹[correctState, correctStateCovariance, smoothState, smoothStateCovariance] = helperGenerateSmoothData(defaultaircraft,measPosTraj,time);
使用该方法可视化前向估计和平滑结果HelpertrajectoryViewer.函数。
HelpertrajectoryViewer（Trajnum，Time，Corricstate，SmoteState，Truestate（：：，trajnum），measpostraj）;
从结果来看，平滑过程表现较差，在120秒左右变得不稳定。这主要是由于IMM滤波器中的过程噪声不足。对于IMM滤波器的恒定加速度和恒定转弯运动模型，在短时间内飞机加速度和急转弯的快速变化要求较高的过程噪声。
调整过程噪声，以更好的平滑性能
在本节中，您将调整滤波器的过程噪声，以获得更好的估计和平滑结果。首先，构造一个恒定速度、恒定加速度和恒定周转率的模型用于IMM滤波器。
constVelocityEKF = initcvekf(检测);constantAccelerationEKF = initcaekf(检测);constantTurnEKF = initctekf(检测);
为了补偿速度不确定性，基于机动目标的速度值的变化的估计，增加所有三个轴中的恒流模型的过程噪声。类似地，为了补偿加速度不确定性，基于制B制造目标的加速度值的变化估计，增加所有三个轴的恒定加速模型的过程噪声。为了补偿转弯率不确定性，增加恒定转弯模型的转弯速率的过程噪声。
ConstantVelocityekf.processnoise = 36 *眼睛（3,3）;Constantaccelerationekf.processnoise = 100 *眼睛（3,3）;constantturnekf.processnoise = 36 *眼睛（4,4）;
使用三个定义的估计过滤器创建IMM滤波器并启用平滑功能。
过滤器= {constVelocityEKF; constantAccelerationEKF; constantTurnEKF};imm = trackingIMM(过滤器,“TransitionProbabilities”, 0.99);imm。EnableSmoothing = true;imm。MaxNumSmoothingSteps =大小(measPosition, 1);
利用该滤波器对6架飞机的轨迹进行逐一估计，得到平滑估计。可视化每个轨迹的结果。
为了i = 1:6 trajNum = i;measPosTraj = measPosition (:,:, trajNum);[correctState, correctStateCovariance, smoothState, smoothStateCovariance] = helperGenerateSmoothData(clone(imm)，measPosTraj,time);%显示每个轨迹的结果HelpertrajectoryViewer（Trajnum，时间，校正静音，Smoothstate，Truestate（：：，trajnum），measpostraj）结尾
从结果，与先前的平滑结果相比，使用调整后的过程噪声的使用大大降低了估计误差，而无需调整过程噪声。
总结
在此示例中，您学习了如何平滑过滤结果以获得更好的状态估计。您还可以了解为特定应用程序正确调整过滤器噪声的重要性。
万博1manbetx支持功能
helperGenerateCarMeasurements
为转弯的汽车轨迹生成测量输入
功能[Moxposition，Truestate，Time] = HelperGenerAtecArmeasurements（）创建一辆汽车转弯的轨迹。加速= [6 2 0;18 4 0;25 7 0;28 10 0;31 15 0;33 22 0];timesteps = [02 4 6 8 10];%的场景。场景=跟踪Cenario（“UpdateRate”，20）;平台=平台（场景）;plat.traptory = WayPointTrajectory（加速度，时间步来）;%使用场景中的路径点创建轨迹。numsteps = 0;trueposition = [];truevelocity = [];trueaciceleration = [];时间= [];尽管advance(scene) = platformPoses(scene);t = scene.SimulationTime;numSteps = numSteps + 1;truePosition (numSteps 1:3) = poses.Position;trueVelocity (numSteps 1:3) = poses.Velocity;trueAcceleration (numSteps 1:3) = poses.Acceleration;时间（numsteps）= t;结尾trueState = [truePosition (: 1:), trueVelocity (: 1:), truePosition (:, 2:), trueVelocity (:, 2:), truePosition (:, 3:), trueVelocity (:, 3:)];％将测量噪声添加到真实位置。= 1* randn(size(truePosition));测量位置= truePosition +[测量噪音(:，1:2)，0 (numSteps,1)];结尾
helperGenerateAircraftTrajMeasurements
为多架飞机的轨迹生成测量输入
功能[measuposition, trueState, time] = helperGenerateAircraftTrajMeasurements()％加载轨迹航点和速度。该文件包含航点的表和用于重建六种飞机轨迹的％速度（以米和米为单位）。负载('benchmarktrajectorytables.mat'，“trajTable”）;%的场景。场景=跟踪Cenario（“UpdateRate”10）;给每个平台分配一个轨迹为了N =1:6 plat =平台(场景);traj = trajTable {n};平台。轨迹= waypointTrajectory (traj。锚点,traj。时间,“速度”, traj.Velocities);结尾%使用场景中的路径点创建轨迹。numsteps = 0;trueposition = [];truevelocity = [];trueaciceleration = [];时间= [];尽管advance(scene) = platformPoses(scene);t = scene.SimulationTime;numSteps = numSteps + 1;位置= Vertcat（Pose.position）;Velocity = Vertcat（Pose.velocity）;加速= Vertcat（Pose.Acceleration）;Trueposition（NumSteps，1：3,1：6）=置换（位置，[3 2 1]）;TrueVelocity（NumSteps，1：3,1：6）=换算（速度，[3 2 1]）;TrueacicHeleration（Numsteps，1：3,1：6）=换算（加速度，[3 2 1]）;时间（numsteps）= t;结尾trueState = [truePosition (: 1:), trueVelocity (: 1:), truePosition (:, 2:), trueVelocity (:, 2:), truePosition (:, 3:), trueVelocity (:, 3:)];%定义测量值为位置，并添加标准偏差为0.1的正态随机噪声。measnoise = 0.1 * randn（尺寸（trueposition））;测量率= TrueCosition + Moynoise;结尾
helperGenerateSmoothData
生成修正和平滑的状态
功能[correctState, correctStateCovariance, smoothState, smoothStateCovariance] = helperGenerateSmoothData(imm, measPosTraj, time)％输出正确和平滑状态numsteps = numel（时间）;positionSelector = [1 0 0 0 0 0; 0 0 1 0 0 0, 0 0 0 0 0 1);InitialState = PositionSelector'* measpostraj（1，:)';initialcovariance = diag（[1,16e4,1,16e4,1,16e4]）;速度没有被测量初始化（IMM，InitialState，InitialCovariance）;numSteps correctState = 0(6日);correctStateCovariance = 0 (6 6 numSteps);ForderModelProb = Zeros（3，NumSteps）;矫正器（1,1）= measpostraj（1,1）;矫正器（3,1）= measpostraj（1,2）;矫正器（5,1）= measpostraj（1,3）;CorressStatecovariance（：，：，1）= imm.statecovariance;ForderModelProb（：，1）= Imm.modelpability;为了i = 2:numSteps dt = time(i) - time(i-1);预测(imm) dt);[correctState(:，i)， correctStateCovariance(:，:，i)] = correct(imm, measPosTraj(i，:));correctModelProb (:, i) = imm.ModelProbabilities;结尾[Smoothstate，SmoothStatecovariance，ShimpleModelprob] =平滑（IMM）;结尾
HelpertrajectoryViewer.
可视化平滑结果并比较RMS错误
功能帮助轨迹查看器(n，时间，correctState, smoothState, trueStateTraj, measPosTraj)%创建图形和两个面板，第一个面板的位置和第二个面板错误可视化的％trueposition = truestatetraj（：，[1 3 5]）';corrictPosition =校正状态（[1 3 5]，:);ShiblePosition = Smoothstate（[1 3 5]，:);cliftPosError =校正器（[1,3,5]，1：端1） - Truestatetraj（1：end -1，[1,3,5]）';smoothposerror = smoothstate（[1,3,5]，:) - truestatetraj（1：end-1，[1,3,5]）';classVelerror =校正器（[2,4,6]，1：端1） - truestatetraj（1：end -1，[2,4,6]）';smoothvelerror = smoothstate（[2,4,6]，:) - truestatetraj（1：end-1，[2,4,6]）';numsteps = numel（时间）;ClassPOSRMS = Zeros（1，NumSteps-1）;Smoothposrms = Zeros（1，Numsteps - 1）; correctVelRMS = zeros(1,numSteps-1); smoothVelRMS = zeros(1,numSteps - 1);为了i = 1：numsteps -1 deltapc = classError（：，i）;corrcleposrms（：，i）= sqrt（（deltapc'）* deltapc）;deltaps = smoothposerror（：，i）;SmoothPosrms（：，i）= sqrt（（deltaps'）* deltaps）;deltavc = classvelerror（：，i）;classvelrms（：，i）= sqrt（（deltavc'）* deltavc）;deltavs = smoothvelerror（：，i）;smoothvelrms（：，i）= sqrt（（deltavs'）* deltavs）;结尾hfig = figure（“名字”，“轨迹”+ n，'numbertitle'，'离开'，“单位”，“归一化”，“位置”，[0.1 0.1 0.8]）;hleftpanel = Uipanel（HFIG，“位置”，[0 0 1/2 1]);hRightPanel = uipanel (hfig,“位置”，[1/2 0 /2 1]);xMin = 10 *地板(min (truePosition (: 1)) / 10 e3) 5;xMax = 10 *装天花板(max (truePosition (: 1)) / 10 e3) + 5;yMin = 10 *地板(min (truePosition (:, 2)) / 10 e3) 5;yMax = 10 *装天花板(max (truePosition (:, 2)) / 10 e3) + 5;zMin = 10 *地板(min (truePosition (:, 3)) / 10 e3) 5;zMax = 10 *装天花板(max (truePosition (:, 3)) / 10 e3) + 5;左侧面板中的x-y绘图％，用于绘制真正的轨迹，纠正％轨迹，平滑轨迹hAx1 =轴('父母'，hleftpanel）;轴（hax1，[xmin xmax ymin yminymax zmin zmax]）;Plot3（Hax1，Trueposition（1，:)，Trueposition（2，:)，Trueposition（3，:)，“- - -”，“线宽”2);抓住在Plot3（Hax1，Measpostraj（：，1），Measpostraj（:,2），Measpostraj（:,3），“。”，“MarkerSize”，3，“线宽”2);Plot3（Hax1，CorrictPosition（1，:)，CorrictPosition（2，:)，CorrictPosition（3，:)，“- - -”，“线宽”, . 05);plot3 (hAx1 smoothPosition (1:), smoothPosition (2:), smoothPosition (3:)“- - -”，“线宽”2);标题(“轨迹”+ n);包含(“X (m)”）;ylabel (“Y (m)”）;Zlabel（“Z (m)”）;lObj =传奇('真正的位置'，测量位置的，“纠正位置”，'平滑位置'，'地点'，“东北”）;轴（HAX1，“广场”）;网格(hAx1,'次要的'）;%如果Z为0，将视图切换到X-YViewSwitch =均值（Trueposition（3，:)）;如果ViewSwitch == 0查看（90,90）;别的查看（60,10）;结尾设置图例位置legendPos = lObj.Position;集(lObj,“位置”，[Legendpos（1）* 1.1 Legendpos（2）Legendpos（3）Legendpos（4）]）集（GCA，'ZDIR'，“反向”）;%位置RMS图hAx2 =次要情节(2,1,1,'父母', hRightPanel);线（HAX2，时间（20：结束-1），CLASSPOSRMS（20：结束），“颜色”，“米”）;抓住在；线(hAx2、时间(20:end-1) smoothPosRMS(20:结束),“颜色”，“b”）;网格(hAx2,“上”）;网格(hAx2,'次要的'）;xlim([1正]);包含('时间（s）'）;ylabel (的位置误差）;传奇(“纠正错误”，'平滑错误'）;%速度RMS图hax3 =子图（2,1,2，'父母', hRightPanel);线(hAx3、时间(20:end-1) correctVelRMS(20:结束),“颜色”，“米”）;抓住在；线(hAx3、时间(20:end-1) smoothVelRMS(20:结束),“颜色”，“b”）;网格(hAx3,“上”）;网格(hAx3,'次要的'）;xlim([1正]);包含('时间（s）'）;ylabel (“速度误差”）;传奇(“纠正错误”，'平滑错误'）;结尾
您还可以从以下列表中选择一个网站：
