def setUpClass(cls) -> None:
cls.pregen = PreGenData()
# Init settings
sigma_z = 3
sigma_a_CV = 0.2
sigma_a_CT = 0.1
sigma_omega = 0.002 * np.pi
cls.ts = 0.1
# Transition matrix
PI = np.array([[0.95, 0.05], [0.05, 0.95]])
assert np.allclose(PI.sum(axis=1), 1), "rows of PI must sum to 1"
measurement_model = CartesianPosition(sigma_z, state_dim=5)
CV = WhitenoiseAccelleration(sigma_a_CV, n=5)
CT = ConstantTurnrate(sigma_a_CT, sigma_omega)
ekf_filters = [EKF(CV, measurement_model), EKF(CT, measurement_model)]
cls.imm_filter = IMM(ekf_filters, PI)
# IMM init weights
init_weights = np.array([0.5] * 2)
init_mean = [0] * 5
init_cov = np.diag(
[1] * 5
) # HAVE TO BE DIFFERENT: use intuition, eg. diag guessed distance to true values squared.
init_mode_states = [GaussParams(init_mean, init_cov)
] * 2 # copy of the two modes
cls.init_immstate = MixtureParameters(init_weights, init_mode_states)
def _(self, init: dict) -> GaussParams:
got_mean = False
got_cov = False
for key in init:
if not got_mean and key in ["mean", "x", "m"]:
mean = init[key]
got_mean = True
if not got_cov and key in ["cov", "P"]:
cov = init[key]
got_cov = True
assert (got_mean and got_cov
), f"EKF do not recognize mean and cov keys in the dict {init}."
return GaussParams(mean, cov)
def mix_states(
self,
immstate: MixtureParameters[MT],
# the mixing probabilities: shape=(M, M)
mix_probabilities: np.ndarray,
) -> List[MT]:
components = immstate.components
means = np.array([comp.mean for comp in components])
covs = np.array([comp.cov for comp in components])
# Columnwise iteration on mix_probs, transponated for columns
mixed_states = [
GaussParams(
*gaussian_mixture_moments(w=mix_prob, mean=means, cov=covs))
for mix_prob in mix_probabilities.T
]
return mixed_states
sigma_a_CT = 0.5
sigma_omega = 0.3
# markov chain
PI11 = 0.9
PI22 = 0.9
p10 = 0.9 # initvalue for mode probabilities
PI = np.array([[PI11, (1 - PI11)], [(1 - PI22), PI22]])
assert np.allclose(np.sum(PI, axis=1), 1), "rows of PI must sum to 1"
mean_init = np.array([0, 0, 0, 0, 0])
cov_init = np.diag([1000, 1000, 30, 30, 0.1])**2 # THIS WILL NOT BE GOOD
mode_probabilities_init = np.array([p10, (1 - p10)])
mode_states_init = GaussParams(mean_init, cov_init)
init_imm_state = MixtureParameters(mode_probabilities_init,
[mode_states_init] * 2)
assert np.allclose(np.sum(mode_probabilities_init),
1), "initial mode probabilities must sum to 1"
# make model
measurement_model = measurementmodels.CartesianPosition(sigma_z, state_dim=5)
dynamic_models: List[dynamicmodels.DynamicModel] = []
dynamic_models.append(dynamicmodels.WhitenoiseAccelleration(sigma_a_CV, n=5))
dynamic_models.append(dynamicmodels.ConstantTurnrate(sigma_a_CT, sigma_omega))
ekf_filters = []
ekf_filters.append(ekf.EKF(dynamic_models[0], measurement_model))
ekf_filters.append(ekf.EKF(dynamic_models[1], measurement_model))
imm_filter = imm.IMM(ekf_filters, PI)
ax1.plot(*Xgt.T[:2])
ax1.scatter(*Z.T[:2])
# %% tune single filters
# parameters
sigma_z = 3
sigma_a_CV = 0.3
sigma_a_CT = 0.1
sigma_omega = 0.002 * np.pi
# initial values
init_mean = np.array([0, 0, 2, 0, 0])
init_cov = np.diag([25, 25, 3, 3, 0.0005])**2
init_state_CV = GaussParams(init_mean[:4],
init_cov[:4, :4]) # get rid of turn rate
init_state_CT = GaussParams(init_mean, init_cov) # same init otherwise
init_states = [init_state_CV, init_state_CT]
# create models
measurement_model_CV = measurementmodels.CartesianPosition(sigma_z)
measurement_model_CT = measurementmodels.CartesianPosition(sigma_z,
state_dim=5)
CV = dynamicmodels.WhitenoiseAccelleration(sigma_a_CV)
CT = dynamicmodels.ConstantTurnrate(sigma_a_CT, sigma_omega)
# create filters
filters = []
filters.append(ekf.EKF(CV, measurement_model_CV))
filters.append(ekf.EKF(CT, measurement_model_CT))
CV = dynamicmodels.WhitenoiseAccelleration(sigma_a_CV, n=state_dim)
CT = dynamicmodels.ConstantTurnrate(sigma_a_CT, sigma_omega)
ekf_filters = [ekf.EKF(CV, measurement_model), ekf.EKF(CT, measurement_model)]
# Transition matrix test
assert np.allclose(PI.sum(axis=1), 1), "rows of PI must sum to 1"
assert PI.shape[0] == modes, 'Dimension transition matrix not same as modes'
# IMM filter instantiation
imm_filter = imm.IMM(ekf_filters, PI)
# IMM init mode probabilities test
assert np.allclose(np.sum(init_weights), 1), \
"initial mode probabilities must sum to 1"
init_mode_states = [GaussParams(init_mean, init_cov)] * modes
init_immstate = MixtureParameters(init_weights, init_mode_states) # Mixture of two mode states
tracker = pda.PDA(imm_filter, clutter_intensity, PD, gate_size)
# allocate
NEES = np.zeros(K)
NEESpos = np.zeros(K)
NEESvel = np.zeros(K)
tracker_update = init_immstate
tracker_update_list = []
tracker_predict_list = []
tracker_estimate_list = []
# estimate
for k, (Zk, x_true_k) in enumerate(zip(Z, Xgt)):
tracker_predict = tracker.predict(tracker_update, Ts=Ts)
def _(self, init: Union[Tuple, List]) -> GaussParams:
return GaussParams(*init)