Skip to content
Snippets Groups Projects
Commit 8149955b authored by Anton Kullberg's avatar Anton Kullberg
Browse files

ipynb: working on JPDA. Enumeration of hypothesis finished.

parent 9e03540c
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
exercise-session-2.ipynb
+ 291
− 68
View file @ 8149955b
%% Cell type:markdown id:7b4f552e-2590-44c2-ad81-13866058fb1e tags: %% Cell type:markdown id:7b4f552e-2590-44c2-ad81-13866058fb1e tags:
## <center> Target Tracking -- Exercise 2 </center> ## <center> Target Tracking -- Exercise 2 </center>
### <center> Anton Kullberg </center> ### <center> Anton Kullberg </center>
This report contains solutions for the second set of exercises in the Target Tracking course given at Linköping University during fall 2021. This report contains solutions for the second set of exercises in the Target Tracking course given at Linköping University during fall 2021.
%% Cell type:code id:9e44486f tags: %% Cell type:code id:9e44486f tags:
``` python ``` python
import numpy as np import numpy as np
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
import seaborn as sns import seaborn as sns
sns.set() sns.set()
rng = np.random.default_rng(13) rng = np.random.default_rng(13)
import jax import jax
import jax.numpy as jnp import jax.numpy as jnp
import tqdm import tqdm
import scipy.stats as stats import scipy.stats as stats
import scipy import scipy
from src.sim import generate_data from src.sim import generate_data
from src.trajectories import get_ex2_trajectories from src.trajectories import get_ex2_trajectories
trajectories = get_ex2_trajectories() trajectories = get_ex2_trajectories()
``` ```
%% Cell type:code id:e5ead4d2-2d44-4ef4-8961-ab8af73f2ba0 tags: %% Cell type:code id:e5ead4d2-2d44-4ef4-8961-ab8af73f2ba0 tags:
``` python ``` python
def generate_data(trajectories, sensor_model, clutter_model, rng=None): def generate_data(trajectories, sensor_model, clutter_model, rng=None):
"""Simulates measurements along a state trajectory according to a sensor and clutter model. """Simulates measurements along a state trajectory according to a sensor and clutter model.
The function assumes Gaussian white noise affecting the measurements. The function assumes Gaussian white noise affecting the measurements.
Parameters Parameters
---------- ----------
trajectories : dict of numpy.ndarrays trajectories : dict of numpy.ndarrays
A dict with entries with nx by N arrays where nx is the state dimension and N is the number of time steps. Sample time T=1 is assumed. A dict with entries with nx by N arrays where nx is the state dimension and N is the number of time steps. Sample time T=1 is assumed.
sensor_model : dict sensor_model : dict
A dictionary with the following entries: A dictionary with the following entries:
h : callable h : callable
measurement function measurement function
R : numpy.ndarray R : numpy.ndarray
measurement covariance measurement covariance
PD : float PD : float
probability of detection probability of detection
clutter_model : dict clutter_model : dict
A dictionary with the following entries: A dictionary with the following entries:
volume : dict volume : dict
A dictionary with xmin, xmax, ymin, ymax (completely rectangular tracking volume) A dictionary with xmin, xmax, ymin, ymax (completely rectangular tracking volume)
lam : float lam : float
Clutter rate of Poisson distributed clutter. (lambda to coincide with numpy/wiki notation) Clutter rate of Poisson distributed clutter. (lambda to coincide with numpy/wiki notation)
rng : Generator rng : Generator
A numpy random number generator. Can be constructed by e.g. np.random.default_rng() A numpy random number generator. Can be constructed by e.g. np.random.default_rng()
Returns Returns
------- -------
list of numpy.ndarray list of numpy.ndarray
Each list item is a numpy.ndarray with zero or more measurements (ny by x) Each list item is a numpy.ndarray with zero or more measurements (ny by x)
""" """
N = max([T.shape[1] for key, T in trajectories.items()]) # Maximum length of a trajectory interesting for this purpose N = max([T.shape[1] for key, T in trajectories.items()]) # Maximum length of a trajectory interesting for this purpose
if rng is None: if rng is None:
rng = np.random.default_rng() rng = np.random.default_rng()
measurements = [] measurements = []
ny = sensor_model['h'](trajectories[next(iter(trajectories))][:, 0]).size # Get the dimensionality of the measurements ny = sensor_model['h'](trajectories[next(iter(trajectories))][:, 0]).size # Get the dimensionality of the measurements
for n in range(N): for n in range(N):
# Determine amount of clutter this time # Determine amount of clutter this time
nclutter = rng.poisson(lam=clutter_model['lam']) nclutter = rng.poisson(lam=clutter_model['lam'])
trajs = [T for key, T in trajectories.items() if n<T.shape[1]] # Figure out what trajectories are active right now trajs = [T for key, T in trajectories.items() if n<T.shape[1]] # Figure out what trajectories are active right now
Ntrajs = len(trajs) # Calc. number of trajectories present in the current time step Ntrajs = len(trajs) # Calc. number of trajectories present in the current time step
# Initialize an array w/ the number of measurements this time step # Initialize an array w/ the number of measurements this time step
cur_measurements = np.empty((ny, nclutter+Ntrajs)) cur_measurements = np.empty((ny, nclutter+Ntrajs))
cur_measurements[:, :] = np.NaN cur_measurements[:, :] = np.NaN
if nclutter != 0: if nclutter != 0:
# Calc. clutter states # Calc. clutter states
clutter_states = np.concatenate([ clutter_states = np.concatenate([
rng.uniform(low=clutter_model['volume']['xmin'], rng.uniform(low=clutter_model['volume']['xmin'],
high=clutter_model['volume']['xmax'], size=(nclutter,)), high=clutter_model['volume']['xmax'], size=(nclutter,)),
rng.uniform(low=clutter_model['volume']['ymin'], rng.uniform(low=clutter_model['volume']['ymin'],
high=clutter_model['volume']['ymax'], size=(nclutter,)) high=clutter_model['volume']['ymax'], size=(nclutter,))
]).reshape(-1, nclutter) ]).reshape(-1, nclutter)
cur_measurements[:, :nclutter] = (sensor_model['h'](clutter_states)+\ cur_measurements[:, :nclutter] = (sensor_model['h'](clutter_states)+\
rng.multivariate_normal(mean=np.zeros((ny,)), cov=sensor_model['R'], size=(nclutter)).squeeze().T).reshape(-1, nclutter) rng.multivariate_normal(mean=np.zeros((ny,)), cov=sensor_model['R'], size=(nclutter)).squeeze().T).reshape(-1, nclutter)
# Generate measurement of target(s) (possibly) # Generate measurement of target(s) (possibly)
for nt, traj in enumerate(trajs): for nt, traj in enumerate(trajs):
if rng.uniform() <= sensor_model['PD']: if rng.uniform() <= sensor_model['PD']:
y = sensor_model['h'](traj[:, n])+\ y = sensor_model['h'](traj[:, n])+\
rng.multivariate_normal(mean=np.zeros((ny,)), cov=sensor_model['R']) rng.multivariate_normal(mean=np.zeros((ny,)), cov=sensor_model['R'])
cur_measurements[:, nclutter+nt] = y.flatten() # Add actual observation to array cur_measurements[:, nclutter+nt] = y.flatten() # Add actual observation to array
cur_measurements = cur_measurements[~np.isnan(cur_measurements)].reshape(ny, -1) # Remove nan measurements (i.e. targets that did not generate a measurement) cur_measurements = cur_measurements[~np.isnan(cur_measurements)].reshape(ny, -1) # Remove nan measurements (i.e. targets that did not generate a measurement)
measurements.append(cur_measurements) measurements.append(cur_measurements)
return measurements return measurements
``` ```
%% Cell type:code id:746fda2d-1e68-4715-9deb-b2afdd324537 tags:
``` python
T = []
trajs = ['T1', 'T3', 'T5', 'T6']
filtered_trajs = {key: T for key, T in trajectories.items() if key in trajs}
```
%% Cell type:code id:ad8aeebb tags: %% Cell type:code id:ad8aeebb tags:
``` python ``` python
import src.gaters as gaters import src.gaters as gaters
import src.filters as filters import src.filters as filters
import src.associators as associators import src.associators as associators
import src.trackers as trackers import src.trackers as trackers
import src.plotters as plotters import src.plotters as plotters
import src.models as models import src.models as models
import src.logic as logic import src.logic as logic
#R = np.diag([10, 0.001])**2 R = np.diag([10, 0.001])**2
R = np.diag([0, 0]) # No noise # R = np.diag([0, 0]) # No noise
PD = 0.99 # Perfect detections PD = 0.9 # Perfect detections
lam = 0 # No clutter lam = 2 # No clutter
volume = dict(xmin=0, xmax=2500, ymin=0, ymax=2000) volume = dict(xmin=0, xmax=2500, ymin=0, ymax=2000)
V = (volume['xmax']-volume['xmin'])*(volume['ymax']-volume['ymin']) V = (volume['xmax']-volume['xmin'])*(volume['ymax']-volume['ymin'])
Bfa = lam/V Bfa = lam/V
Bfa = 4e-7 # Bfa = 4e-7
sensor_model = models.radar_model(R=R, PD=PD) sensor_model = models.radar_model(R=R, PD=PD)
clutter_model = dict(volume=volume, lam=lam, Bfa=Bfa) clutter_model = dict(volume=volume, lam=lam, Bfa=Bfa)
Bnt = Bfa Bnt = Bfa
q = 1 q = 10
motion_model = models.cv_model(Q=q*np.identity(2), D=2, T=1) motion_model = models.cv_model(Q=q*np.identity(2), D=2, T=1)
# Setup Gater # Setup Gater
gamma = 4.7 gamma = 4.7
gater = gaters.MahalanobisGater(sensor_model, gamma) gater = gaters.MahalanobisGater(sensor_model, gamma)
P0 = np.diag([10, 10, 1000, 1000]) P0 = np.diag([10, 10, 1000, 1000])
Ptm = 0.01 Ptm = 0.01
Pfc = 0.001 Pfc = 0.001
lam = 0.9 lam = 0.7
logic_params = dict(PD=sensor_model['PD'], PG=1, lam=lam, Ptm=Ptm, Pfc=Pfc, Bfa=0.05, Ldel=np.log(Ptm/(1-Pfc))) logic_params = dict(PD=sensor_model['PD'], PG=1, lam=lam, Ptm=Ptm, Pfc=Pfc, Bfa=Bfa, Ldel=0.1*np.log(Ptm/(1-Pfc)), Bnt=Bnt)
def init_track(y, k, identity, filt): def init_track(y, k, identity, filt):
track = dict(stage='tentative', score=0, x=[], P=[], t=[k], identity=identity, associations=[k], filt=filt) track = dict(stage='tentative', Lt=0, x=[], P=[], t=[k], identity=identity, associations=[k], filt=filt)
x0 = np.concatenate(plotters.radar_to_pos(y)).flatten() x0 = np.concatenate(plotters.radar_to_pos(y[:, None])).flatten()
x0 = np.hstack([x0, np.zeros((2,))]) x0 = np.hstack([x0, np.zeros((2,))])
track['x'] = [x0] track['x'] = [x0]
track['P'] = [P0] track['P'] = [P0]
return track return track
filt = filters.EKF(motion_model, sensor_model) filt = filters.EKF(motion_model, sensor_model)
``` ```
%% Cell type:code id:746fda2d-1e68-4715-9deb-b2afdd324537 tags: %% Cell type:code id:1ff81504-d524-4106-ba87-4d0242930ea0 tags:
``` python ``` python
T = [] import pdb
trajs = ['T1', 'T3', 'T5', 'T6'] class JPDA():
filtered_trajs = {key: T for key, T in trajectories.items() if key in trajs} def __init__(self, logic, logic_params, init_track, filt, gater, clutter_model):
self.logic = logic
self.logic_params = logic_params
self.init_track = init_track
self.filt = filt
self.gater = gater
self.clutter_model = clutter_model
def _update_track(self, meas, track):
if meas.size == 0:
track = self.logic(np.array([]), track['filt'], track, self.logic_params) # If no meas associated, still update logic of track
return
# Calculate prediction error of each measurement
yhat = track['filt'].sensor_model['h'](track['x'][-1])
eps = meas-yhat
track = self.logic(meas, track['filt'], track, self.logic_params)
# Update
track['x'][-1], track['P'][-1] = track['filt'].update(track['x'][-1], track['P'][-1], eps)
def evaluate(self, Y):
tracks = [] # Store all tracks
confirmed_tracks = [] # Store the confirmed tracks (for plotting purposes only)
ids = 0
for k, meas_k in tqdm.tqdm(enumerate(Y), desc="Evaluating observations: "):
ny = meas_k.shape[1]
unused_meas = np.ones((ny), dtype=bool)
live_tracks = [track for track in confirmed_tracks if track['stage']=='confirmed']
# if live_tracks:
# association_matrix, _ = get_association_matrix(meas_k, live_tracks, self.logic_params, self.gater)
# # Solve association problem
# row_ind, col_ind = scipy.optimize.linear_sum_assignment(-association_matrix)
# for row, col in zip(row_ind, col_ind):
# if col >= len(live_tracks): # No target to associate the measurement to
# continue
# else:
# unused_meas[row] = 0 # Remove this measurement from further consideration
# # Update confirmed tracks
# self._update_track(meas_k[:, row], live_tracks[col])
# live_tracks[col]['associations'].append(k) # If we've associated something, add the time here (for plotting purposes)
# for i in range(len(live_tracks)):
# if i not in col_ind:
# self._update_track(np.array([]), live_tracks[i])
tentative_tracks = [track for track in tracks if track['stage'] == 'tentative']
if tentative_tracks:
association_matrix, validation_matrix = trackers.get_association_matrix(meas_k[:, unused_meas], tentative_tracks, self.logic_params, self.gater)
Nc = len(tentative_tracks)
rec = compute_prob(association_matrix[:, :ny+Nc], validation_matrix, self.logic_params)
pdb.set_trace()
# Solve association problem
row_ind, col_ind = scipy.optimize.linear_sum_assignment(-association_matrix)
meas = meas_k[:, unused_meas]
for row, col in zip(row_ind, col_ind):
if col >= len(tentative_tracks): # No target to associate the measurement to
continue
else:
unused_meas[(meas_k == meas[:,[row]]).all(axis=0)] = 0 # Remove this measurement from consideration
# Update confirmed tracks
self._update_track(meas[:, row], tentative_tracks[col])
tentative_tracks[col]['associations'].append(k) # If we've associated something, add the time here (for plotting purposes)
if tentative_tracks[col]['stage'] == 'confirmed':
confirmed_tracks.append(tentative_tracks[col]) # If a track has been confirmed, add it to confirmed tracks
for i in range(len(tentative_tracks)):
if i not in col_ind:
self._update_track(np.array([]), tentative_tracks[i])
# Use the unused measurements to initiate new tracks
for meas in meas_k[:, unused_meas].T:
tracks.append(self.init_track(meas, k, ids, self.filt))
ids += 1
for track in tracks:
if track['stage'] != 'deleted':
x, P = track['filt'].propagate(track['x'][-1], track['P'][-1])
track['x'].append(x)
track['P'].append(P)
track['t'].append(k+1)
return tracks, confirmed_tracks
def compute_prob(association_matrix, validation_matrix, logic_params):
# Association matrix is assumed to consist of tracks and FA, no NT.
ny = association_matrix.shape[0]
ntracks = association_matrix.shape[1]-ny
P = np.zeros((ny, ntracks))
def rec_find_associations(association_matrix, assoc_done, logic_params):
inds = np.where(association_matrix[0, :] != -np.inf)[0] # These are the nodes necessary to look at
this_assoc = []
for k, i in enumerate(inds):
if i not in assoc_done:
if association_matrix.shape[0] != 1:
assoc = rec_compute_prob(association_matrix[1:, :], [[i]], logic_params)
this_assoc.extend(assoc)
else:
this_assoc.append([i])
result = []
for assoc in assoc_done:
for th_assoc in this_assoc:
result.append(assoc + th_assoc)
return result
possible_associations = rec_find_associations(association_matrix, [[]], logic_params) # Recursively finds possible measurement hypothesis
pdb.set_trace()
return res
```
%% Cell type:code id:31e8ca55-8bb0-4662-977a-857d8cdf4ea2 tags:
``` python
jpda = JPDA(logic.score_logic, logic_params, init_track, filt, gater, clutter_model)
Y = generate_data(filtered_trajs, sensor_model, clutter_model, rng)
tracks, confirmed_tracks = jpda.evaluate(Y)
``` ```
%% Cell type:code id:0c29b3fb-55ac-4c19-9acb-b493e536bd8d tags:
``` python
from src.utility import match_tracks_to_ground_truth
matches = match_tracks_to_ground_truth(confirmed_tracks, filtered_trajs)
```
%% Cell type:code id:5dc32f5b-53c4-40e8-8c4b-9c50274cf2f0 tags:
``` python
fig, ax = plt.subplots(3, 1, figsize=(16, 15))
Yt = np.hstack(Y)
yx, yy = plotters.radar_to_pos(Yt)
for key, T in filtered_trajs.items():
ax[1].plot(T[0, :], T[1, :], color='k', lw=3)
for track in tracks:
x = np.vstack(track['x'])
t = np.hstack(track['t']).flatten()
assoc = np.hstack(track['associations']).flatten()
if track in confirmed_tracks:
ls = '-'
l = ax[0].plot(t, track['identity']*np.ones(t.shape), ls=ls, markersize=3)[0]
ax[0].plot(assoc, track['identity']*np.ones(assoc.shape), ls='', color=l.get_color(), marker='x', markersize=6)
ax[1].plot(x[:, 0], x[:, 1], ls=ls, color=l.get_color(), lw=2)
else:
ls = '--'
ax[1].plot(x[:, 0], x[:, 1], ls=ls, lw=2)
ax[0].set_ylabel('Track identity')
ax[0].set_title('Confirmed tracks over time')
ax[0].set_xlabel('Time index, k')
ax[1].plot(yx, yy, '.', color='k')
ax[1].set_xlabel(r'$p_x$')
ax[1].set_ylabel(r'$p_y$')
ax[1].set_title('Measurements and measurement predictions + tracks')
# Plot the RMSE for the matched trajectories
for track_id, key in matches.items():
T = filtered_trajs[key]
track = [track for track in confirmed_tracks if track['identity'] == track_id][0]
x = np.vstack(track['x']).T
tf = np.hstack(track['t']).flatten()[-1]
gtf = T.shape[1]
if T.shape[1] > x.shape[1]:
N = x.shape[1]
else:
N = T.shape[1]
xrmse = np.sum((T[:, :N]-x[:2, :N])**2, axis=0)/N
terr = np.abs(gtf-tf)
T = np.linspace(0, x.shape[1]/N, N)
ax[2].plot(T, xrmse, label='{} -- Time error: {} timesteps'.format(key, terr))
ax[2].set_xlabel('Trajectory index')
ax[2].set_ylabel('Positional RMSE')
ax[2].legend()
plt.tight_layout()
plt.show()
```
%% Output
%% Cell type:code id:c6ab09ff-ae92-4df2-bda4-2f81535ef319 tags: %% Cell type:code id:c6ab09ff-ae92-4df2-bda4-2f81535ef319 tags:
``` python ``` python
import pdb import pdb
confirmed_tracks = [] # Store the confirmed tracks (for plotting purposes only) confirmed_tracks = [] # Store the confirmed tracks (for plotting purposes only)
tracks = [] # Store all tracks tracks = [] # Store all tracks
Y = generate_data(filtered_trajs, sensor_model, clutter_model, rng)
confirmed_tracks.append(init_track(Y[0][:,[0]], 0, 0, filt))
confirmed_tracks[-1]['stage']='confirmed'
ids = 0 ids = 0
for k, meas_k in tqdm.tqdm(enumerate(Y), desc="Evaluating observations: "): for k, meas_k in tqdm.tqdm(enumerate(Y), desc="Evaluating observations: "):
ny = meas_k.shape[1] ny = meas_k.shape[1]
unused_meas = np.ones((1, ny), dtype=bool) unused_meas = np.ones((ny), dtype=bool)
live_tracks = [track for track in confirmed_tracks if track['stage']=='confirmed'] live_tracks = [track for track in confirmed_tracks if track['stage']=='confirmed']
Nc = len(live_tracks) # Number of confirmed tracks still alive
validation_matrix = np.zeros((ny, Nc), dtype=bool)
association_matrix = -np.inf*np.ones((ny, Nc+2*ny))
# Entry for false alarms
np.fill_diagonal(association_matrix[:, Nc:Nc+ny], np.log(clutter_model['Bfa']))
# Entry for new targets
np.fill_diagonal(association_matrix[:, Nc+ny:], np.log(Bnt))
for ti, track in enumerate(live_tracks): # Iterate over confirmed tracks
validation_matrix[:, ti] = gater.gate(track['x'][-1], track['P'][-1], meas_k)
# Entry for validated tracks
val_meas = meas_k[:, validation_matrix[:, ti]] # Get the validated measurements for this track
yhat = track['filt'].sensor_model['h'](track['x'][-1]) # Calculate the predicted measurement for this track
H = track['filt'].sensor_model['dhdx'](track['x'][-1])
py = stats.multivariate_normal.pdf(val_meas.squeeze().T, mean=yhat.flatten(), cov=H@track['P'][-1]@H.T+track['filt'].sensor_model['R'])
association_matrix[validation_matrix[:, ti], ti] = np.log(track['filt'].sensor_model['PD']*py/(1-track['filt'].sensor_model['PD'])) # PG assumed = 1
# Solve association problem
row_ind, col_ind = scipy.optimize.linear_sum_assignment(-association_matrix)
pdb.set_trace()
if live_tracks:
association_matrix = get_association_matrix(meas_k, live_tracks, gater)
# Solve association problem
row_ind, col_ind = scipy.optimize.linear_sum_assignment(-association_matrix)
for row, col in zip(row_ind, col_ind):
if col >= len(live_tracks): # No target to associate the measurement to
continue
else:
unused_meas[row] = 0 # Remove this measurement from further consideration
# Update confirmed tracks
update_track(meas_k[:, row], live_tracks[col], logic.score_logic, logic_params)
live_tracks[col]['associations'].append(k) # If we've associated something, add the time here (for plotting purposes)
for i in range(len(live_tracks)):
if i not in col_ind:
live_tracks[i] = logic.score_logic(np.array([]), live_tracks[i]['filt'], live_tracks[i], logic_params) # If no meas associated, still update logic of track
# Update confirmed tracks
update_track(meas_k, unused_meas, filt, track, associator, gater, logic, logic_params)
if tmp.sum() != unused_meas.sum(): # If we've associated something, add the time here (for plotting purposes)
track['associations'].append(k)
tentative_tracks = [track for track in tracks if track['stage'] == 'tentative'] tentative_tracks = [track for track in tracks if track['stage'] == 'tentative']
for track in tentative_tracks:
if track['stage'] == 'deleted': if tentative_tracks:
continue association_matrix = get_association_matrix(meas_k[:, unused_meas], tentative_tracks, gater)
tmp = unused_meas.copy() # Solve association problem
# Update tentative tracks row_ind, col_ind = scipy.optimize.linear_sum_assignment(-association_matrix)
update_track(meas_k, unused_meas, filt, track, associator, gater, logic, logic_params) meas = meas_k[:, unused_meas]
if tmp.sum() != unused_meas.sum(): # If we've associated something, add the time here (for plotting purposes) for row, col in zip(row_ind, col_ind):
track['associations'].append(k) if col >= len(tentative_tracks): # No target to associate the measurement to
if track['stage'] == 'confirmed': continue
confirmed_tracks.append(track) # If a track has been confirmed, add it to confirmed tracks else:
unused_meas[(meas_k == meas[:,[row]]).all(axis=0)] = 0 # Remove this measurement from consideration
# Used the unused measurements to initiate new tracks # Update confirmed tracks
for meas in meas_k[unused_meas]: update_track(meas[:, row], tentative_tracks[col], logic.score_logic, logic_params)
tracks.append(init_track(meas, k, ids)) tentative_tracks[col]['associations'].append(k) # If we've associated something, add the time here (for plotting purposes)
if tentative_tracks[col]['stage'] == 'confirmed':
confirmed_tracks.append(tentative_tracks[col]) # If a track has been confirmed, add it to confirmed tracks
for i in range(len(tentative_tracks)):
if i not in col_ind:
tentative_tracks[i] = logic.score_logic(np.array([]), tentative_tracks[i]['filt'], tentative_tracks[i], logic_params) # If no meas associated, still update logic of track
# Use the unused measurements to initiate new tracks
for meas in meas_k[:, unused_meas].T:
tracks.append(init_track(meas, k, ids, filt))
ids += 1 ids += 1
for track in tracks: for track in tracks:
if track['stage'] != 'deleted': if track['stage'] != 'deleted':
x, P = filt.propagate(track['x'][-1], track['P'][-1]) x, P = track['filt'].propagate(track['x'][-1], track['P'][-1])
track['x'].append(x) track['x'].append(x)
track['P'].append(P) track['P'].append(P)
track['t'].append(k+1) track['t'].append(k+1)
``` ```
%% Output
Evaluating observations: : 102it [00:33, 3.04it/s]
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment