Coordinated turn model, 'ct_demo'
In this demonstration program the IMM filter is used in a scenario, in which one of the models is non-linear. The used model is the so called coordinated turn model, which extends the Wiener velocity with an additional turn rate parameter, whose value determines the speed of the turn and it's sign the direction.
In this case the model is used to generate a trajectory with two turns having opposite directions. The trajectory as well as the linearly generated position measurements are shown in figure 1.
The figure 2 shows the filtered and smoothed position estimates by using IMM-EKF and IMM-UKF by using the turning model and a standard Wiener process velocity model. For comparison the figure also shows the estimates produced by the Kalman filter combined with the Wiener process velocity model. Especially the estimates of both IMM smoother are clearly superior over the ones of Kalman smoother.
The figure 3 shows the filtered and smoothed estimates of the turning model's probability on each time step. The filtered and smoothed estimates for the turn rate parameter are shown in figure 4. Notice how the smoothing considerably improves the overall estimation performance on both cases. It also reduces the lag present in the filtered estimates.
See the documentation for further discussion and estimation performance comparison.
Files used in this example:
Dynamic model function for the coordinated turn model
Jacobian of the coordinated turn model's dynamic model
Inverse dynamics of the coordinated turn model
Coordinated Turn Model demonstration