sigma_a_CV = 1.
sigma_a_CV_high = 10
sigma_a_CT = 1.
sigma_omega = 3e-3 * np.pi
# markov chain
p10 = 0.9 # initvalue for mode probabilities
PI = np.array([[0.85, 0.15], [0.15, 0.85]])
PI = np.array([[0.9, 0.05, 0.05], [0.05, 0.9, 0.05], [0.15, 0.15, 0.7]])
assert np.allclose(np.sum(PI, axis=1), 1), "rows of PI must sum to 1"
mean_init = np.array([*Xgt[0, :], 0])
cov_init = np.diag([20, 20, 2, 2, 0.0001])**2
mode_probabilities_init = np.array([0.8, 0.1, 0.1])
mode_states_init = GaussParams(mean_init, cov_init)
init_imm_state = MixtureParameters(mode_probabilities_init,
[mode_states_init] * 3)
init_ekf_state = GaussParams(mean_init, cov_init)
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))
dynamic_models.append(
dynamicmodels.WhitenoiseAccelleration(sigma_a_CV_high, n=5))
ekf_filters = []
ekf_filters.append(ekf.EKF(dynamic_models[0], measurement_model))
ekf_filters.append(ekf.EKF(dynamic_models[1], measurement_model))
ekf_filters.append(ekf.EKF(dynamic_models[2], measurement_model))
imm_filter = imm.IMM(ekf_filters, PI)
def _(self, init: Union[Tuple, List]) -> GaussParams:
return GaussParams(*init)
ax1.scatter(*Z.T[:2]) # %% tune single filters # parameters sigma_z = 2.25 sigma_a_CV = 0.22 sigma_a_CT = 0.07 sigma_omega = 0.0016 * 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)) # allocate
def init_filter_state(self, init_state: "ET_like"):
return GaussParams(init_state['mean'], init_state['cov'])
sigma_a = 2.6
sigma_z = 3.1
# create the model and estimator object
dynmod = dynamicmodels.WhitenoiseAccelleration(sigma_a)
measmod = measurementmodels.CartesianPosition(sigma_z)
ekf_filter = ekf.EKF(dynmod, measmod)
print(ekf_filter)
# Optimal init for model
mean = np.array([*Z[1], *(Z[1] - Z[0]) / Ts])
cov11 = sigma_z**2 * np.eye(2)
cov12 = sigma_z**2 * np.eye(2) / Ts
cov22 = (2 * sigma_z**2 / Ts**2 + sigma_a**2 * Ts / 3) * np.eye(2)
cov = np.block([[cov11, cov12], [cov12.T, cov22]])
init_ekfstate = GaussParams(mean, cov)
ekfpred_list = []
ekfupd_list = []
ekfupd = init_ekfstate
NIS = np.empty(K)
NEES_pred = np.empty(K)
NEES_upd = np.empty(K)
dists_pred = np.empty((K, 2))
dists_upd = np.empty((K, 2))
# estimate
for k, (zk, x_true_k) in enumerate(zip(Z[2:], Xgt[2:])):
ekfpred = ekf_filter.predict(ekfupd, Ts)
ekfupd = ekf_filter.update(zk, ekfpred)
NIS[k] = ekf_filter.NIS(zk, ekfpred)
tracker = pda.PDA(ekf_filter, clutter_intensity, PD, gate_size)
# allocate
NEES = np.zeros(K)
NEESpos = np.zeros(K)
NEESvel = np.zeros(K)
# initialize
x_bar_init = np.array([*Z[0][true_association[0] - 1], 0, 0])
P_bar_init = np.zeros((4, 4))
P_bar_init[[0, 1], [0, 1]] = 2 * sigma_z**2
P_bar_init[[2, 3], [2, 3]] = 10**2
init_state = GaussParams(
x_bar_init, P_bar_init
) # tracker.init_filter_state({"mean": x_bar_init, "cov": P_bar_init})
tracker_update = init_state
tracker_update_list = []
tracker_predict_list = []
# estimate
print("Start estimating")
for k, (Zk, x_true_k) in enumerate(zip(Z, Xgt)):
tracker_predict = tracker.predict(tracker_update, Ts) # TODO
tracker_update = tracker.update(Zk, tracker_predict) # TODO
NEES[k] = (tracker_update.mean - x_true_k[0:4]) @ la.solve(
tracker_update.cov, tracker_update.mean - x_true_k[0:4]
) # tracker.state_filter.NEESes(tracker_update, x_true_k) TODO
NEESpos[k] = (tracker_update.mean[0:2] - x_true_k[0:2]) @ la.solve(
tracker_update.cov[0:2, 0:2], tracker_update.mean[0:2] - x_true_k[0:2]
# allocate
NEES = np.zeros(K)
NEESpos = np.zeros(K)
NEESvel = np.zeros(K)
# initialize
x_bar_init = np.array([*Z[0][true_association[0] - 1], 0, 0])
P_bar_init = np.zeros((4, 4))
P_bar_init[[0, 1], [0, 1]] = 2 * sigma_z ** 2
P_bar_init[[2, 3], [2, 3]] = 10 ** 2
#removed because of post in the forum
#init_state = tracker.init_filter_state({"mean": x_bar_init, "cov": P_bar_init})
init_state = GaussParams(x_bar_init, P_bar_init)
tracker_update = init_state
tracker_update_list = []
tracker_predict_list = []
# estimate
for k, (Zk, x_true_k) in enumerate(zip(Z, Xgt)):
tracker_predict = tracker.predict(tracker_update, x_true_k, Ts)# TODO
tracker_update = tracker.update(tracker_update, Zk, tracker_predict)# TODO
NEES[k] = tracker.NEES(tracker_update, x_true_k)# TODO
NEESpos[k] = tracker.NEES(tracker_update[:1], x_true_k[:1])# TODO
NEESvel[k] = tracker.NEES(tracker_update[1:], x_true_k[1:])# TODO
tracker_predict_list.append(tracker_predict)
tracker_update_list.append(tracker_update)
# 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)) trackers = [] trackers.append(pda.PDA(ekf_filters[0], clutter_intensity, PD, gate_size)) # EKF CV trackers.append(pda.PDA(ekf_filters[1], clutter_intensity, PD, gate_size)) # EKF CT names = ["CV_EKF", "CT_EKF"] init_ekf_state = GaussParams(mean_init, cov_init) # init_imm_pda_state = tracker.init_filter_state(init_ekf_state) NEES = np.zeros(K) NEESpos = np.zeros(K) NEESvel = np.zeros(K) tracker_update_init = [init_ekf_state, init_ekf_state] tracker_update_list = np.empty((len(trackers), len(Xgt)), dtype=MixtureParameters) tracker_predict_list = np.empty((len(trackers), len(Xgt)), dtype=MixtureParameters) tracker_estimate_list = np.empty((len(trackers), len(Xgt)), dtype=MixtureParameters) # estimate Ts = np.insert(Ts,0, 0., axis=0) x_hat = np.empty((len(trackers), len(Xgt), 5)) prob_hat = np.empty((len(trackers), len(Xgt), 2))
elif model == "CT":
# sensor
sigma_z = 9
clutter_intensity = 1e-5
PD = 0.8
gate_size = 2
#dynamic model
sigma_a_CT = 3.5
sigma_omega = 0.05
#Fører bruk av bare ett filter med CT modellen til dårlig filter consistency?
mean_init = np.array([7116, 3617, 0, 0, 0])
cov_init = np.diag([14, 14, 2, 2, 0.01])**2
ekf_init = GaussParams(mean_init, cov_init)
# make model
measurement_model = measurementmodels.CartesianPosition(sigma_z, state_dim=5)
if model == "CV":
ekf_filter = ekf.EKF(
dynamicmodels.WhitenoiseAccelleration(sigma_a_CV, n=5),
measurement_model)
elif model == "CT":
ekf_filter = ekf.EKF(
dynamicmodels.ConstantTurnrate(sigma_a_CT, sigma_omega),
measurement_model)
tracker = pda.PDA(ekf_filter, clutter_intensity, PD, gate_size)
# allocate
NEES = np.zeros(K)
NEESpos = np.zeros(K)
NEESvel = np.zeros(K)
# initialize
x_bar_init = np.array([*Z[0][true_association[0] - 1], 0, 0])
x_bar_init = np.array([30, -70, 0, 0])
P_bar_init = np.zeros((4, 4))
P_bar_init[[0, 1], [0, 1]] = 2 * sigma_z**2
P_bar_init[[2, 3], [2, 3]] = 20**2
#init_state = tracker.init_filter_state({"mean": x_bar_init, "cov": P_bar_init})
init_state = GaussParams(x_bar_init, P_bar_init)
tracker_update = init_state
tracker_update_list = []
tracker_predict_list = []
# estimate
for k, (Zk, x_true_k) in enumerate(zip(Z, Xgt)):
tracker_predict = tracker.predict(tracker_update, Ts)
tracker_update = tracker.update(Zk, tracker_predict)
NEES[k] = ekf_filter.NEES(tracker_update, x_true_k[0:4])
x, P = tracker_update
NEESpos[k] = ekf_filter.NEES(GaussParams(x[0:2], P[0:2, 0:2]),
x_true_k[0:2])
NEESvel[k] = ekf_filter.NEES(GaussParams(x[2:4], P[2:4, 2:4]),
x_true_k[2:4])
P_bar_init[[0, 1], [0, 1]] = 2 * sigma_z**2
P_bar_init[[2, 3], [2, 3]] = 10**2
init_state = tracker.init_filter_state({"mean": x_bar_init, "cov": P_bar_init})
tracker_update = init_state
tracker_update_list = []
tracker_predict_list = []
# estimate
for k, (Zk, x_true_k) in enumerate(zip(Z, Xgt)):
tracker_predict = tracker.predict(tracker_update, Ts)
tracker_update = tracker.update(Zk, tracker_predict)
NEES[k] = ekf_filter.NEES(tracker_update, x_true_k[0:4])
x, P = tracker_update
NEESpos[k] = ekf_filter.NEES(GaussParams(x[0:2], P[0:2, 0:2]), \
x_true_k[0:2])
NEESvel[k] = ekf_filter.NEES(GaussParams(x[2:4], P[2:4, 2:4]), \
x_true_k[2:4])
tracker_predict_list.append(tracker_predict)
tracker_update_list.append(tracker_update)
x_hat = np.array([upd.mean for upd in tracker_update_list])
# calculate a performance metric
posRMSE = np.sqrt(np.mean((x_hat[:, 0:2] - Xgt[:, 0:2])**2))
velRMSE = np.sqrt(np.mean((x_hat[:, 2:4] - Xgt[:, 2:4])**2))
# %% plots
fig3, ax3 = plt.subplots(num=3, clear=True)
raise NotImplementedError
fig1, ax1 = plt.subplots(num=1, clear=True)
ax1.plot(*Xgt.T[:2])
ax1.scatter(*Z.T[:2])
# %% tune single filters
sigma_z = 8.7
sigma_a_CT = 0.2
sigma_a_CV = 5.5
sigma_omega = 5e-6 * np.pi
init_state = {
"mean": np.array(Xgt[0, :]),
"cov": np.diag([1, 1, 1, 1, 0.005])**2
}
init_state = GaussParams(init_state["mean"], init_state["cov"])
measurement_model = measurementmodels.CartesianPosition(
sigma_z, state_dim=5) # note the 5
CV = dynamicmodels.WhitenoiseAccelleration(sigma_a_CV, n=5) # note the 5
CT = dynamicmodels.ConstantTurnrate(sigma_a_CT, sigma_omega)
# make models
filters = []
filters.append(ekf.EKF(CV, measurement_model))
filters.append(ekf.EKF(
CT, measurement_model)) #, so you understand what is going on here
pred = []
upd = []
stats = []