From b49c65bd868bfb5c736db2d758061183e399e997 Mon Sep 17 00:00:00 2001
From: Anton Kullberg <anton.kullberg@liu.se>
Date: Mon, 1 Nov 2021 14:46:14 +0100
Subject: [PATCH] py: updated data file loading
---
exercise-1.py | 60 ++++++++++++++++++++--------------------
exercise-session-1.ipynb | 2 +-
2 files changed, 31 insertions(+), 31 deletions(-)
diff --git a/exercise-1.py b/exercise-1.py
index a3c038d..46f25f6 100644
--- a/exercise-1.py
+++ b/exercise-1.py
@@ -3,7 +3,7 @@
# ## <center> Target Tracking -- Exercise 1 </center>
# ### <center> Anton Kullberg </center>
-#
+#
# This report contains solutions for the first set of exercises in the Target Tracking course given at Linköping University during fall 2021.
# In[1]:
@@ -63,7 +63,7 @@ def h(x): # Assumes positional coordinates first
target_range = jnp.linalg.norm(xt[:2, :], axis=0) # Use JAX numpy to be able to auto-differentiate
target_bearing = jnp.arctan2(xt[1, :], xt[0, :])
return jnp.vstack([target_range, target_bearing]).squeeze()
-
+
R = np.diag([10, 0.001])**2
PD = 0.9
lam = 2
@@ -87,11 +87,11 @@ associator = associators.NNAssociator()
# ### Task 2.1 - EKF for a low maneuvering target
-#
-# A constant velocity model was implemented w/ process noise tuning $\mathbf{Q}=\mathbf{I}$.
-#
+#
+# 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.
@@ -114,11 +114,11 @@ plt.show()
# ### Task 2.2 - EKF for a high maneuvering target
-#
-# A constant velocity model was implemented w/ process noise tuning $\mathbf{Q}=100\mathbf{I}$.
-#
+#
+# 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.
@@ -141,12 +141,12 @@ plt.show()
# ### Task 2.3 - IMM for Maneuvering Targets
-#
-# 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.
+#
+# 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.
# The transition probability matrix was set to
# $$\Pi = \begin{bmatrix}p & 1-p\\1-p & p\end{bmatrix},$$
# with $p=0.9$, which yielded good performance.
-#
+#
# 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.
@@ -208,7 +208,7 @@ plt.show()
# ### Task 2.4 - Mysterious Data
-#
+#
# 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}$.
@@ -216,7 +216,7 @@ plt.show()
from scipy.io import loadmat
-dat24 = loadmat('ex1data.mat')
+dat24 = loadmat('data/ex1data.mat')
# In[14]:
@@ -273,7 +273,7 @@ plt.show()
# 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)
# $$N1=2,~M1=2,~N2=2,~M2=3,~N3=3$$
@@ -283,15 +283,15 @@ plt.show()
def update_track(meas_k, unused_meas, filt, track, associator, gater, logic, logic_params):
# Calculate prediction error of each measurement
yhat = filt.sensor_model['h'](track['x'][-1])
-
+
eps = meas_k[unused_meas]-yhat
-
+
if eps.size == 0:
track = logic(np.array([]), filt, track, logic_params)
return
if eps.ndim < 2:
eps = np.expand_dims(eps, 0)
-
+
# Gating step
accepted_meas = gater.gate(track['x'][-1], track['P'][-1], eps)
# If any measurements are accepted, select the nearest one
@@ -308,7 +308,7 @@ def update_track(meas_k, unused_meas, filt, track, associator, gater, logic, log
unused_meas[unused_meas] = tmp
# Update
track['x'][-1], track['P'][-1] = filt.update(track['x'][-1], track['P'][-1], eps[:, accepted_meas][:, yind])
-
+
else:
track = logic(np.array([]), filt, track, logic_params)
@@ -398,12 +398,12 @@ N = len(Y)
P0 = np.diag([R[0, 0], 0.1])
logic_params = dict(N1=2, M1=2, N2=2, M2=3, N3=3)
filt = filters.EKF(motion_model, sensor_model)
-init_track = lambda y, k, identity: dict(stage='tentative',
- nmeas=1,
- nass=1,
- x=[np.array([y,0])],
- P=[P0],
- t=[k],
+init_track = lambda y, k, identity: dict(stage='tentative',
+ nmeas=1,
+ nass=1,
+ x=[np.array([y,0])],
+ P=[P0],
+ t=[k],
identity=identity,
associations=[k])
@@ -419,7 +419,7 @@ plt.show()
# 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
# $$P_{TM}=0.01,~P_{FC}=0.001,~\lambda=0.9,~P_D=0.9,~P_G=1,~\beta_{FA}=0.05,~L_{hi}=\log\frac{1-P_{TM}}{P_{FC}},~L_{lo}=\log\frac{P_{TM}}{1-P_{FC}},~L_{del}=L_{lo}$$
@@ -432,9 +432,9 @@ lam = 0.9
logic_params = dict(PD=sensor_model['PD'], PG=1, lam=lam, Ptm=Ptm, Pfc=Pfc, Bfa=0.05, Ldel=np.log(Ptm/(1-Pfc)))
init_track = lambda y, k, identity: dict(stage='tentative',
Lt=0,
- x=[np.array([y,0])],
- P=[P0],
- t=[k],
+ x=[np.array([y,0])],
+ P=[P0],
+ t=[k],
identity=identity,
associations=[k])
diff --git a/exercise-session-1.ipynb b/exercise-session-1.ipynb
index a96bfad..780e1c8 100644
--- a/exercise-session-1.ipynb
+++ b/exercise-session-1.ipynb
@@ -293,7 +293,7 @@
"outputs": [],
"source": [
"from scipy.io import loadmat\n",
- "dat24 = loadmat('ex1data.mat')"
+ "dat24 = loadmat('data/ex1data.mat')"
]
},
{
--
GitLab