# Smooth Trajectory Estimation of `trackingIMM` Filter

This example shows how to smooth state estimates of a target using the `smooth` object function. Smoothing is a technique to refine previous state estimates using the up-to-date measurements and the state estimate information. In this example, you will learn how to improve previously corrected estimates from an Interacting Multi-Model (IMM) filter by running a backward recursion, which produces smoothed and more accurate state estimates. In the first section, you implement a smooth algorithm to smooth the trajectory of a turning car. In the remainder of this example, you perform smoothing on several highly maneuvering aircraft trajectories, taken from the Benchmark Trajectories for Multi-Object Tracking example.

### Fixed Interval Smoothing to Track Turning Car

In this section, you smooth the trajectory estimate of a turning car. First, you use the `helperGenerateCarMeasurements` function to generate the position measurements of the car and the corresponding truth.

```rng(2021); % For repeatable results % Generate measurements for a car taking a turn [measPositionCar, trueStateCar, timeCar] = helperGenerateCarMeasurements(); % Define the initial detection in the format of objectDetection detection = objectDetection(0, [0;0;0], 'MeasurementNoise', eye(3,3));```

Using the `initekfimm` initialization function, you create an IMM filter based on the constant velocity, constant acceleration, and constant turn motion models. To setup the filter for backward smoothing, you set the `EnableSmoothing` property of the filter to `true`. Since you want to perform fixed interval smoothing (offline smoothing) on the state estimates, which utilizes all corrected and predicted steps, you set the `MaxNumSmoothingSteps` property to the number of measurement steps.

```defaultIMMCar = initekfimm(detection); % Enable Smoothing defaultIMMCar.EnableSmoothing = true; defaultIMMCar.MaxNumSmoothingSteps = size(measPositionCar,1); % Initilaize IMM positionSelector = [1 0 0 0 0 0;0 0 1 0 0 0;0 0 0 0 1 0]; % Select position from state initialState = positionSelector' * measPositionCar(1,:)'; initialCovariance = diag([1,1e4,1,1e4,1,1e4]); initialize(defaultIMMCar, initialState, initialCovariance);```

To obtain the forward estimates of the car, you run the filter by iteratively predicting and correcting the state estimates.

```% Initialize the corrected states numSteps = numel(timeCar); correctState = zeros(6,numSteps); correctStateCovariance = zeros(6,6,numSteps); correcState(:,1) = defaultIMMCar.State; correctStateCovariance(:,:,1) = defaultIMMCar.StateCovariance; % Forward tracking with prediction and correction for i = 2:numSteps dt = timeCar(i) - timeCar(i-1); predict(defaultIMMCar, dt); [correctState(:,i), correctStateCovariance(:,:,i)] = correct(defaultIMMCar, measPositionCar(i,:)); end```

To perform the smoothing, simply call the `smooth` object function of the filter. The function returns the smoothed states, state covariance, and model probabilities.

`[smoothState, smoothStateCovariance, modelProbabilities] = smooth(defaultIMMCar);`

Next, use the `helperTrajectoryViewer` function to visualize the smooth results and the RMS errors. The results show that using offline smoothing of an IMM filter enables you to reduce the errors in estimates.

```trajNum = 0; helperTrajectoryViewer(trajNum,timeCar,correctState, smoothState, trueStateCar, measPositionCar);```

### Smooth Highly Maneuvering Aircraft Trajectories with Default IMM Configuration

In this section, you smooth the trajectories of six highly maneuvering aircraft. The trajectories used in this section are the same as those in the Benchmark Trajectories for Multi-Object Tracking example. In the example, the aircraft acceleration changes as much as 35 $\mathrm{m}/{\mathrm{s}}^{2}$ during some of the maneuvers. You use the `helperGenerateAircraftTrajMeasurements` function to generate the measurement data and truth.

`[measPosition, trueState, time] = helperGenerateAircraftTrajMeasurements;`

Configurate an IMM filter similar as the previous section.

```% Define initial detection detection = objectDetection(0, [0;0;0], 'MeasurementNoise', eye(3,3)); defaultIMMAircraft = initekfimm(detection); % Enable Smoothing defaultIMMAircraft.EnableSmoothing = true; defaultIMMAircraft.MaxNumSmoothingSteps = size(measPosition,1);```

Using the `helperGenerateSmoothData` function, you run the created IMM filter, obtain the corrected state estimates, and generate the smoothed state estimates for the first trajectory.

```% Only estimate the first trajectory trajNum = 1; % Extract measurement data measPosTraj = measPosition(:,:,trajNum); % Estimate and smooth the trajectory using the helperGenerateSmoothData function [correctState, correctStateCovariance, smoothState, smoothStateCovariance] = helperGenerateSmoothData(defaultIMMAircraft,measPosTraj,time);```

Visualize the forward estimation and smoothing results using the `helperTrajectoryViewer` funciton.

`helperTrajectoryViewer(trajNum,time,correctState, smoothState, trueState(:,:,trajNum), measPosTraj);`

As seen from the results, the smoothing process performs poorly and becomes unstable near 120 seconds. This is mainly due to insufficient process noise in the IMM filter. Rapid changes in aircraft acceleration and sharp turns in short intervals requires higher process noise for the constant acceleration and constant turn motion models of the IMM filter.

### Adjusting Process Noise for Better Smoothing Performance

In this section, you adjust the process noise of the filter for better estimation and smoothing results. First, construct a constant velocity, a constant acceleration, and a constant turn-rate model to be used in the IMM filter.

```constVelocityEKF = initcvekf(detection); constantAccelerationEKF = initcaekf(detection); constantTurnEKF = initctekf(detection);```

To compensate the velocity uncertainty, increase the process noise of the constant velocity model in all three axes based on the estimates of the change in the velocity values of the maneuvering target. Similarly, to compensate the acceleration uncertainty, increase the process noise for the constant acceleration model in all three axes based on the estimate of the change in the acceleration values of the manuvering target. To compensate the turning rate uncertainty, increase the process noise for the turn rate of the constant-turn model.

```constantVelocityEKF.ProcessNoise = 36*eye(3,3); constantAccelerationEKF.ProcessNoise = 100*eye(3,3); constantTurnEKF.ProcessNoise = 36*eye(4,4);```

Create an IMM filter using the three defined estimation filters and enable the smoothing capability.

```filters = {constVelocityEKF;constantAccelerationEKF;constantTurnEKF}; imm = trackingIMM(filters,'TransitionProbabilities', 0.99); imm.EnableSmoothing = true; imm.MaxNumSmoothingSteps = size(measPosition,1);```

Use the filter to estimate the trajectories of six aircraft one-by-one and obtain the smoothed estimates. Visualize the results for each trajectory.

```for i = 1:6 trajNum = i; measPosTraj = measPosition(:,:,trajNum); [correctState, correctStateCovariance, smoothState, smoothStateCovariance] = helperGenerateSmoothData(clone(imm),measPosTraj,time); % Visualize the results for each trajectory helperTrajectoryViewer(trajNum, time, correctState, smoothState, trueState(:,:,trajNum), measPosTraj) end```

From the results, the use of the adjusted process noise greatly reduces the estimation errors compared with the previous smoothing results without adjusting the process noise.

### Summary

In this example, you learned how to smooth filter results to obtain better state estimates. You also learn the importance of adjusting filter noise properly for your specific applications.

### Supporting Functions

#### `helperGenerateCarMeasurements`

Generate measurement inputs for the trajectory of a turning car

```function [measPosition, trueState, time] = helperGenerateCarMeasurements() % Create a trajectory of a car taking a turn. carWayPoints = [6 2 0; 18 4 0; 25 7 0; 28 10 0; 31 15 0; 33 22 0]; timeSteps = [0 2 4 6 8 10]; % Scenario Generation. scene = trackingScenario('UpdateRate',20); plat = platform(scene); plat.Trajectory = waypointTrajectory(carWayPoints, timeSteps); % Create trajectory using waypoints from the scenario. numSteps = 0; truePosition = []; trueVelocity = []; trueAcceleration = []; time = []; while advance(scene) poses = 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; time(numSteps) = t; end trueState = [truePosition(:,1,:),trueVelocity(:,1,:),truePosition(:,2,:),trueVelocity(:,2,:),truePosition(:,3,:),trueVelocity(:,3,:)]; % Add measurement noise to true position. measNoise = 1* randn(size(truePosition)); measPosition = truePosition + [measNoise(:,1:2), zeros(numSteps,1)]; end```

#### helperGenerateAircraftTrajMeasurements

Generate measurement inputs for the trajectories of multiple aircraft

```function [measPosition, trueState, time] = helperGenerateAircraftTrajMeasurements() % Load trajectory waypoints and velocities. The file contains tables of waypoints and % velocities (in units of meters and meters per second) that are used to reconstruct six aircraft trajectories. load('benchmarkTrajectoryTables.mat', 'trajTable'); % Scenario generation. scene = trackingScenario('UpdateRate',10); % Assign each platform with a trajectory for n=1:6 plat = platform(scene); traj = trajTable{n}; plat.Trajectory = waypointTrajectory(traj.Waypoints, traj.Time, 'Velocities', traj.Velocities); end % Create trajectory using waypoints from the scenario. numSteps = 0; truePosition = []; trueVelocity = []; trueAcceleration = []; time = []; while advance(scene) poses = platformPoses(scene); t = scene.SimulationTime; numSteps = numSteps + 1; position = vertcat(poses.Position); velocity = vertcat(poses.Velocity); acceleration = vertcat(poses.Acceleration); truePosition(numSteps,1:3,1:6) = permute(position,[3 2 1]); trueVelocity(numSteps,1:3,1:6) = permute(velocity,[3 2 1]); trueAcceleration(numSteps,1:3,1:6) = permute(acceleration,[3 2 1]); time(numSteps) = t; end trueState = [truePosition(:,1,:),trueVelocity(:,1,:),truePosition(:,2,:),trueVelocity(:,2,:),truePosition(:,3,:),trueVelocity(:,3,:)]; % Define the measurements to be the position and add normal random noise with a standard deviation of 0.1 to the measurements. measNoise = 0.1* randn(size(truePosition)); measPosition = truePosition + measNoise; end```

#### helperGenerateSmoothData

Generate corrected and smoothed states

```function [correctState, correctStateCovariance, smoothState, smoothStateCovariance] = helperGenerateSmoothData(imm, measPosTraj, time) % Output correct and smooth states numSteps = numel(time); positionSelector = [1 0 0 0 0 0;0 0 1 0 0 0;0 0 0 0 1 0]; initialState = positionSelector' * measPosTraj(1,:)'; initialCovariance = diag([1,16e4,1,16e4,1,16e4]); % Velocity is not measured initialize(imm, initialState, initialCovariance); correctState = zeros(6,numSteps); correctStateCovariance = zeros(6,6,numSteps); correctModelProb = zeros(3,numSteps); correctState(1,1) = measPosTraj(1,1); correctState(3,1) = measPosTraj(1,2); correctState(5,1) = measPosTraj(1,3); correctStateCovariance(:,:,1) = imm.StateCovariance; correctModelProb(:,1) = imm.ModelProbabilities; for i = 2:numSteps dt = time(i) - time(i-1); predict(imm, dt); [correctState(:,i), correctStateCovariance(:,:,i)] = correct(imm, measPosTraj(i,:)); correctModelProb(:,i) = imm.ModelProbabilities; end [smoothState, smoothStateCovariance, smoothModelProb] = smooth(imm); end```

#### `helperTrajectoryViewer`

Visualize smoothing results and compare RMS error

```function helperTrajectoryViewer(n, time, correctState, smoothState, trueStateTraj, measPosTraj) % Create figure and two panels, first panel for position and second panel % for error visualization truePosition = trueStateTraj(:,[1 3 5])'; correctPosition = correctState([1 3 5],:); smoothPosition = smoothState([1 3 5],:); correctPosError = correctState([1,3,5],1:end-1) - trueStateTraj(1:end-1,[1,3,5])'; smoothPosError = smoothState([1,3,5],:) - trueStateTraj(1:end-1,[1,3,5])'; correctVelError = correctState([2,4,6],1:end-1) - trueStateTraj(1:end-1,[2,4,6])'; smoothVelError = smoothState([2,4,6],:) - trueStateTraj(1:end-1,[2,4,6])'; numSteps = numel(time); correctPosRMS = zeros(1,numSteps-1); smoothPosRMS = zeros(1,numSteps - 1); correctVelRMS = zeros(1,numSteps-1); smoothVelRMS = zeros(1,numSteps - 1); for i = 1:numSteps -1 deltaPc = correctPosError(:,i); correctPosRMS(:,i) = sqrt((deltaPc')*deltaPc); deltaPs = smoothPosError(:,i); smoothPosRMS(:,i) = sqrt((deltaPs')*deltaPs); deltaVc = correctVelError(:,i); correctVelRMS(:,i) = sqrt((deltaVc')*deltaVc); deltaVs = smoothVelError(:,i); smoothVelRMS(:,i) = sqrt((deltaVs')*deltaVs); end hfig = figure('Name',"Trajectory " + n,'NumberTitle','off','Units','normalized','Position',[0.1 0.1 0.8 0.8]); hLeftPanel = uipanel(hfig,'Position',[0 0 1/2 1]); hRightPanel = uipanel(hfig,'Position',[1/2 0 1/2 1]); xMin = 10*floor(min(truePosition(:,1))/10e3)-5; xMax = 10*ceil(max(truePosition(:,1))/10e3)+5; yMin = 10*floor(min(truePosition(:,2))/10e3)-5; yMax = 10*ceil(max(truePosition(:,2))/10e3)+5; zMin = 10*floor(min(truePosition(:,3))/10e3)-5; zMax = 10*ceil(max(truePosition(:,3))/10e3)+5; % Place x-y plot in left panel for plotting true trajectory, corrected % trajectory, and smoothed trajectory hAx1 = axes('Parent',hLeftPanel); axis(hAx1,[xMin xMax yMin yMax zMin zMax]); plot3(hAx1,truePosition(1,:),truePosition(2,:),truePosition(3,:),'-','lineWidth',2); hold on plot3(hAx1,measPosTraj(:,1),measPosTraj(:,2),measPosTraj(:,3),'.','MarkerSize',3,'lineWidth',2); plot3(hAx1,correctPosition(1,:),correctPosition(2,:),correctPosition(3,:),'-','lineWidth',.05); plot3(hAx1,smoothPosition(1,:),smoothPosition(2,:),smoothPosition(3,:),'-','lineWidth',2); title("Trajectory " + n); xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); lObj = legend('True Position', 'Measured Position', 'Corrected Position', 'smoothed Position','location',"northeast"); axis(hAx1,'square'); grid(hAx1,'minor'); % Switch view to X-Y if Z 0 viewSwitch = mean(truePosition(3,:)); if viewSwitch == 0 view(90,90); else view(60,10); end % Set legend position legendPos = lObj.Position; set(lObj,'Position',[legendPos(1)*1.1 legendPos(2) legendPos(3) legendPos(4)]) set(gca,'ZDir','reverse'); % Position RMS plot hAx2 = subplot(2,1,1,'Parent',hRightPanel); line(hAx2, time(20:end-1),correctPosRMS(20:end),'color','m'); hold on; line(hAx2, time(20:end-1),smoothPosRMS(20:end),'color','b'); grid(hAx2,'on'); grid(hAx2,'minor'); xlim([1 inf]); xlabel('time (s)'); ylabel('Position Error'); legend('Correct Error', 'Smooth Error'); % Velocity RMS plot hAx3 = subplot(2,1,2,'Parent',hRightPanel); line(hAx3, time(20:end-1),correctVelRMS(20:end),'color','m'); hold on; line(hAx3, time(20:end-1),smoothVelRMS(20:end),'color','b'); grid(hAx3,'on'); grid(hAx3,'minor'); xlim([1 inf]); xlabel('time (s)'); ylabel('Velocity Error'); legend('Correct Error', 'Smooth Error'); end```