PI33 = 0.95 # CV_high -> CV_high
p10 = 0.8 # initvalue for mode probabilities (starter ikke så ofte med en sving,men vet ikke når det dukker opp)
p11 = 0.1
p12 = 0.1
PI = np.array([[PI11, PI12, PI13], [PI21, PI22, PI23], [PI31, PI32, PI33]])
assert np.allclose(np.sum(PI, axis=1), 1), "rows of PI must sum to 1"
mean_init = Xgt[0]
mean_init = np.append(mean_init, 0.1)
cov_init = np.diag([sigma_z*10, sigma_z*10, 0.1, 0.1, 0.1]) ** 2
mode_probabilities_init = np.array([p10, p11, p12])
mode_states_init = GaussParams(mean_init, cov_init)
init_imm_state = MixtureParameters(mode_probabilities_init, [mode_states_init] * 3)
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))
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))
# make model
measurement_model = measurementmodels.CartesianPosition(sigma_z, state_dim=5)
CV = dynamicmodels.WhitenoiseAccelleration(sigma_a_CV, n=5)
CT = dynamicmodels.ConstantTurnrate(sigma_a_CT, sigma_omega)
ekf_filters: List[StateEstimator[GaussParams]] = []
ekf_filters.append(ekf.EKF(CV, measurement_model))
ekf_filters.append(ekf.EKF(CT, measurement_model))
imm_filter: imm.IMM[GaussParams] = imm.IMM(ekf_filters, PI)
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
init_immstate = MixtureParameters(init_weights, init_mode_states)
imm_preds = []
imm_upds = []
imm_ests = []
updated_immstate = init_immstate
for zk in Z:
predicted_immstate = imm_filter.predict(updated_immstate, Ts)
updated_immstate = imm_filter.update(zk, predicted_immstate)
estimate = imm_filter.estimate(updated_immstate)
imm_preds.append(predicted_immstate)
imm_upds.append(updated_immstate)
imm_ests.append(estimate)
x_est = np.array([est.mean for est in imm_ests])
def reduce_mixture(
self, immstate_mixture: MixtureParameters[MixtureParameters[MT]]
) -> MixtureParameters[MT]:
"""
Approximate a mixture of immstates as a single immstate.
We have Pr(a), Pr(s | a), p(x| s, a).
- Pr(a) = immstate_mixture.weights
- Pr(s | a=j) = immstate_mixture.components[j].weights
- p(x | s=i, a=j) = immstate_mixture.components[j].components[i] # ie. Gaussian parameters
So p(x, s) = sum_j Pr(a=j) Pr(s| a=j) p(x| s, a=j),
which we want as a single probability Gaussian pair. Multiplying the above with
1 = Pr(s)/Pr(s) and moving the denominator a little we have
p(x, s) = Pr(s) sum_j [ Pr(a=j) Pr(s| a=j)/Pr(s) ] p(x| s, a=j),
where the bracketed term is Bayes for Pr(a=j|s). Thus the mode conditioned state estimate is.
p(x | s) = sum_j Pr(a=j| s) p(x| s, a=j)
That is:
- we need to invoke discrete Bayes one time and
- reduce self.filter[s].reduce_mixture for each s
"""
# raise NotImplementedError # TODO remove this when done
# extract probabilities as array
## eg. association weights/beta: Pr(a)
weights = immstate_mixture.weights
## eg. the association conditioned mode probabilities element [j, s] is for association j and mode s: Pr(s | a = j)
component_conditioned_mode_prob = np.array(
[c.weights.ravel() for c in immstate_mixture.components])
# flip conditioning order with Bayes to get Pr(s), and Pr(a | s)
mode_prob, mode_conditioned_component_prob = discretebayes.discrete_bayes(
weights, component_conditioned_mode_prob) # TODO
# We need to gather all the state parameters from the associations for mode s into a
# single list in order to reduce it to a single parameter set.
# for instance loop through the modes, gather the paramters for the association of this mode
# into a single list and append the result of self.filters[s].reduce_mixture
# The mode s for association j should be available as imm_mixture.components[j].components[s]
modeAmount = len(immstate_mixture.components[0].weights)
associationAmount = len(immstate_mixture.weights)
mode_indexed_association_mixture = []
for modeIt in range(modeAmount):
currentWeights = []
currentComponents = []
for asscIt in range(associationAmount):
currentWeights.append(mode_conditioned_component_prob[modeIt,
asscIt])
currentComponents.append(
immstate_mixture.components[asscIt].components[modeIt])
mode_indexed_association_mixture.append(
MixtureParameters(np.array(currentWeights), currentComponents))
mode_states: List[GaussParams] = [
self.filters[sidx].reduce_mixture(modeMixture) for sidx,
modeMixture in enumerate(mode_indexed_association_mixture)
] # TODO
immstate_reduced = MixtureParameters(mode_prob, mode_states)
return immstate_reduced
def _(self, init: Sequence) -> MixtureParameters[MT]:
weights = self.initial_mode_probabilities
components = self.init_components(init)
return MixtureParameters(weights, components)
assert np.allclose(np.sum(PI1, axis=1), 1), "rows of PI must sum to 1"
=======
PI11 = 0.95
PI22 = 0.95
PI33 = 0.95 #NADIA
>>>>>>> Stashed changes
PI2 = np.array([[PI11, 3*(1 - PI11)/4, (1-PI11)/4],[3*(1 - PI22)/4, PI22, (1-PI22)/4], [3*(1-PI33)/4, (1-PI33)/4, PI33]])
# init values
mean_init = np.array([7096, 3627, 0, 0, 0])
cov_init = np.diag([10, 10, 20, 20, 0.1]) ** 2
mode_probabilities_init1 = np.array([p10, (1 - p10)])
mode_states_init = GaussParams(mean_init, cov_init)
init_imm_state1 = MixtureParameters(mode_probabilities_init1, [mode_states_init] * 2)
mode_probabilities_init2 = np.array([0.34, 0.33, 0.33]) #arbitrary doesnt have much effect
init_imm_state2 = MixtureParameters(mode_probabilities_init2, [mode_states_init] * 3)
assert np.allclose(
np.sum(mode_probabilities_init1), 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))
<<<<<<< Updated upstream
dynamic_models.append(dynamicmodels.WhitenoiseAccelleration(sigma_a_CV_H, n=5))
PI32 = 0.175
PI33 = 0.7
PI = np.array([
[PI11, PI12, PI13],
[PI21, PI22, PI23],
[PI31, PI32, PI33]
])
assert np.allclose(np.sum(PI, axis=1), 1), "rows of PI must sum to 1"
mean_init = np.array([7116, 3617, 0, 0, 0]) # Sverre: er omtrent der sporet begynner.
cov_init = np.diag([14, 14, 2, 2, 0.01]) ** 2
mode_probabilities_init = np.array([0.7, 0.1, 0.2]) #sverre: utvidet pga den tredje moden
mode_states_init = GaussParams(mean_init, cov_init)
init_imm_state = MixtureParameters(mode_probabilities_init, [mode_states_init] * 3) #sverre: må ganges med tre og ikke to pga. den tredje moden
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_high, n=5)) #five states: two for position, two for velocity and one for angle velocity
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))
ekf_filters.append(ekf.EKF(dynamic_models[2], measurement_model))