# A constant velocity model was implemented w/ process noise tuning $\mathbf{Q}=\mathbf{I}$.
#
# This gave a good performance for trajectory T1 and T3, but very bad results for the concatenated trajectory T4, where the filter completely fails to follow the observations (due to gating, the associations fall off completely and the filter resorts to the CV model, seen by the completely straight trajectory).
#
# 10 simulations were run and the RMSE computed for $p_x$ and $p_y$ independently. The top figure illustrate the filtered trajectories in dashed colored lines, the ground truth trajectory in black and measurements from **one** of the simulations as black dots. The bottom figure illustrates the RMSE over the 10 different simulations.
# As illustrated in the bottom figure, the RMSE explodes because of the complete failure to follow the circular part of the trajectory.
# ### Task 2.2 - EKF for a high maneuvering target
#
# A constant velocity model was implemented w/ process noise tuning $\mathbf{Q}=100\mathbf{I}$.
#
# This gave a good performance for trajectory T2 and somewhat reasonable results for the concatenated trajectory T4. However, the trajectory along the parts given by T1 and T3 are still better with the low maneuvering EKF.
#
# 10 simulations were run and the RMSE computed for $p_x$ and $p_y$ independently. The top figure illustrate the filtered trajectories in dashed colored lines, the ground truth trajectory in black and measurements from **one** of the simulations as black dots. The bottom figure illustrates the RMSE over the 10 different simulations.
# As illustrated in the bottom figure, the RMSE is quite reasonable, but can still be improved.
# A two-model IMM was implemented with two CV models with process noise tuning $\mathbf{Q}_1=\mathbf{I}$ and $\mathbf{Q}_2=100\mathbf{I}$, respectively. Mahalanobis gating was used with $\gamma=15$. Further, a nearest neighbour association step was used for associating the gated measurements.
# 10 simulations were run and the RMSE computed for $p_x$ and $p_y$ independently. Three figures are produced. The top figure illustrates the filtered trajectories in dashed colored lines, the ground truth trajectory in black and measurements from **one** of the simulations as black dots. The middle figure illustrates the RMSE over the 10 different simulations.
# The bottom figure illustrates the model (mode) probabilities over the normalized trajectory length (normalized by the length of the trajectory).
# As illustrated in the middle figure, the RMSE is better than for the two individual models from before. The mode clearly switches in the circular part of the trajectory as can be seen in the bottom figure.
# An IMM was used with two CV models with tuning $\mathbf{Q}_1=\mathbf{I}$ and $\mathbf{Q}_2=1000\mathbf{I}$. Mahalanobis gating was used with $\gamma=4.7$. The transition probability is as in 2.3, with $p=0.8$.
# After looking at the measurements, the first two measurements "seem" to belong to a target and as such, they were used to initialize the target state. The initial uncertainty is set quite high to $\mathbf{P}_0=100\mathbf{I}$.
# The top figure shows the estimated trajectory overlayed over the measurements. The method seems to capture the trajectory well. In the bottom figure, the mode probabilities are visualized. Clearly, the high-maneuvering mode is active in the segments of the trajectory where the target quickly steers.
# ### Task 3.1 - N/M Logic
#
# An N/M logic was implemented with N1/M1 & N2/M2 for confirmation and N3/M3 for termination. N1, M1, N2, M2, N3 are tuneable parameters. M3 is assumed fixed to N3. Mahalanobis gating is used and nearest neighbour association. Further, an EKF is used with the model provided in the exercise. A track is initiated at the current measurement with speed 0 and initial uncertainty given by $\mathbf{P}_0=\mathrm{diag}[R,~0.1]$. The logic parameters were set to (according to the exercise)
# The top figure shows tracks over time. The confirmed tracks are plotted with solid lines and tentative tracks are dashed. Times when measurements are associated to a certain track are visualized as crosses. The bottom figure shows the actual tracks over time. The y-axis are the measurements as well as the predicted measurements. The solid lines are confirmed tracks and dashed are tentative. There seems to be ~6 confirmed tracks over time and quite a few tentative.
# ### Task 3.2 - Score Logic
#
# A score logic was implemented with an exponential forgetting factor to avoid integrator wind-up. The parameters were set to
# The top figure shows tracks over time. The confirmed tracks are plotted with solid lines and tentative tracks are dashed. Times when measurements are associated to a certain track are visualized as crosses. The bottom figure shows the actual tracks over time. The y-axis are the measurements as well as the predicted measurements. The solid lines are confirmed tracks and dashed are tentative. There seems to be ~5 confirmed tracks over time and quite a few tentative. This score-based logic doesn't kill off possible tracks as quickly as the N/M logic (but that is just up to tuning of the deletion criteria as well as the exponential forgetting factor).
