def reduce_mixture(
self, immstate_mixture: MixtureParameters[MixtureParameters[MT]]
) -> MixtureParameters[MT]:
"""Approximate a mixture of immstates as a single immstate"""
# extract probabilities as array
weights = immstate_mixture.weights
component_conditioned_mode_prob = np.array(
[c.weights.ravel() for c in immstate_mixture.components])
# flip conditioning order with Bayes
mode_prob, mode_conditioned_component_prob = discrete_bayes(
weights, component_conditioned_mode_prob)
# Hint list_a of lists_b to list_b of lists_a: zip(*immstate_mixture.components)
mode_states = []
components = zip(
*[comp.components for comp in immstate_mixture.components])
for filt, mode_conditioned, component in zip(
self.filters, mode_conditioned_component_prob, components):
state = filt.reduce_mixture(
MixtureParameters(mode_conditioned, component))
mode_states.append(state)
immstate_reduced = MixtureParameters(mode_prob, mode_states)
return immstate_reduced
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
"""
# 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]
# p(x| s=i, a=j) = imm_mixture.components[j].components[s]
# The sum over a (7.54 in the book) should be left for the reduction in filters[s].reduce_mixture
# => sum(a)(p(sIa)*p(xIs,a))
#p(sIa = j) = imm_mixture.components[j].weights[s]
#p(xIs,a) = imm_mixture.components[j].components[s]
mode_states: list[GaussParams] = [
fs.reduce_mixture(MixtureParameters(modestate_conditional_combined_probability, mode_state_comp))
for fs, modestate_conditional_combined_probability,mode_state_comp in zip(self.filters,
mode_conditioned_component_prob, zip(*[comp.components for comp in immstate_mixture.components]))
]
# TODO
immstate_reduced = MixtureParameters(mode_prob, mode_states)
return immstate_reduced
def update(
self,
# measurements of shape=(M, m)=(#measurements, dim)
Z: np.ndarray,
filter_state: ET,
*,
sensor_state: Optional[Dict[str, Any]] = None,
) -> ET: # The filter_state updated by approximating the data association
"""
Perform the PDA update cycle.
Gate -> association probabilities -> conditional update -> reduce mixture.
"""
# remove the not gated measurements from consideration
gated = # TODO
Zg = Z[gated]
# find association probabilities
beta = # TODO
# find the mixture components
filter_state_updated_mixture_components = # TODO
# make mixture
filter_state_update_mixture = MixtureParameters(
beta, filter_state_update_mixture_components
)
# reduce mixture
filter_state_updated_reduced = # TODO
return filter_state_updated_reduced
def predict(
self,
immstate: MixtureParameters[MT],
# sampling time
Ts: float,
) -> MixtureParameters[MT]:
"""
Predict the immstate Ts time units ahead approximating the mixture step.
Ie. Predict mode probabilities, condition states on predicted mode,
appoximate resulting state distribution as Gaussian for each mode, then predict each mode.
"""
# Basically call above functions and organize
# Step 1
predicted_mode_probability, mixing_probability = self.mix_probabilities(
immstate=immstate, Ts=Ts)
# Step 2
mixed_mode_states: List[MT] = self.mix_states(
immstate, mix_probabilities=mixing_probability)
# Step 3
predicted_mode_states = self.mode_matched_prediction(
mode_states=mixed_mode_states, Ts=Ts)
predicted_immstate = MixtureParameters(predicted_mode_probability,
predicted_mode_states)
return predicted_immstate
def _(self, init: dict) -> MixtureParameters[MT]:
# extract weights
got_weights = False
got_components = False
for key in init:
if not got_weights and key in [
"weights",
"probs",
"probabilities",
"mode_probs",
]:
weights = np.asfarray([key])
got_weights = True
elif not got_components and key in ["components", "modes"]:
components = self.init_components(init[key])
got_components = True
if not got_weights:
weights = self.initial_mode_probabilities
if not got_components:
components = self.init_components(init)
assert np.allclose(weights.sum(),
1), "Mode probabilities must sum to 1 for"
return MixtureParameters(weights, components)
def predict(
self,
immstate: MixtureParameters[MT],
# sampling time
Ts: float,
) -> MixtureParameters[MT]:
"""
Predict the immstate Ts time units ahead approximating the mixture step.
Ie. Predict mode probabilities, condition states on predicted mode,
appoximate resulting state distribution as Gaussian for each mode, then predict each mode.
"""
# TODO: proposed structure
predicted_mode_probability, mixing_probability = self.mix_probabilities(
immstate, Ts) # TODO
mixed_mode_states: List[MT] = self.mix_states(
immstate, mixing_probability) # TODO
predicted_mode_states = self.mode_matched_prediction(
mixed_mode_states, Ts) # TODO
predicted_immstate = MixtureParameters(predicted_mode_probability,
predicted_mode_states)
return predicted_immstate
def update(
self,
Z: np.ndarray,
filter_state: ET,
sensor_state: Optional[Dict[str, Any]] = None,
) -> ET: # The filter_state updated by approximating the data association
"""
Perform the PDA update cycle.
Gate -> association probabilities -> conditional update -> reduce mixture.
"""
# remove the not gated measurements from consideration
gated = self.gate(Z, filter_state, sensor_state)
Zg = Z[gated]
# find association probabilities
beta = self.association_probabilities(Zg, filter_state, sensor_state)
# find the mixture components
filter_state_updated_mixture_components = self.conditional_update(
Zg, filter_state, sensor_state=sensor_state)
# make mixture
filter_state_update_mixture = MixtureParameters(
beta, filter_state_updated_mixture_components)
# reduce mixture
filter_state_updated_reduced = self.reduce_mixture(
filter_state_update_mixture)
return filter_state_updated_reduced
def predict( #sverre: implementation of 6.33? Returnerer en prediksjon for hele IMM'en.
self,
immstate: MixtureParameters[MT],
# sampling time
Ts: float,
) -> MixtureParameters[MT]:
"""
Predict the immstate Ts time units ahead approximating the mixture step.
Ie. Predict mode probabilities, condition states on predicted mode,
appoximate resulting state distribution as Gaussian for each mode, then predict each mode.
"""
predicted_mode_probability, mixing_probability = self.mix_probabilities(
immstate, Ts
)
mixed_mode_states: List[MT] = self.mix_states(immstate, mixing_probability)
predicted_mode_states = self.mode_matched_prediction(mixed_mode_states, Ts)
predicted_immstate = MixtureParameters(
predicted_mode_probability, predicted_mode_states
)
return predicted_immstate
def _(self, init: tuple) -> MixtureParameters[MT]:
assert isinstance(init[0], Sized) and len(init[0]) == len(
self.filters
), f"To initialize from tuple the first element must be of len(self.filters)={len(self.filters)}"
weights = np.asfarray(init[0])
components = self.init_compontents(init[1])
return MixtureParameters(weights, components)
def reduce_mixture(
self,
immstate_mixture: MixtureParameters[MixtureParameters[
MT]] #this is posterior density of x_k (double mixture)
) -> MixtureParameters[MT]:
"""
Approximate a mixture of immstates as a single immstate.
That is:
- we need to invoke discrete Bayes one time and
- reduce self.filter[s].reduce_mixture for each s
"""
# Association weights/beta as array
weights = immstate_mixture.weights # Pr{a}
# Association conditioned mode probabilities
component_conditioned_mode_prob = np.array(
[c.weights.ravel() for c in immstate_mixture.components] # Pr{s|a}
)
# input: Pr(a), Pr(s|a)
# output: Pr(s), Pr(a|s)
mode_prob, mode_conditioned_component_prob = discretebayes.discrete_bayes(
weights, component_conditioned_mode_prob)
num_modes = len(self.filters)
mode_states: List[GaussParams] = [
] # state params from associations for mode s
for (s, pr_a_given_s) in zip(range(num_modes),
mode_conditioned_component_prob):
#~The mode s for association j should be available as imm_mixture.components[j].components[s]
# gather all state params from associations for mode s into a list:
mode_cond_params = np.array(
[c.components[s] for c in immstate_mixture.components])
#gaussian mix with weights Pr(a|s)
mixture_params = MixtureParameters(pr_a_given_s, mode_cond_params)
#reduce and append
mode_states.append(self.filters[s].reduce_mixture(mixture_params))
immstate_reduced = MixtureParameters(mode_prob, mode_states)
return immstate_reduced
def mix_states(
self,
immstate: MixtureParameters[MT],
# the mixing probabilities: shape=(M, M)
mix_probabilities: np.ndarray,
) -> List[MT]:
mixed_states = [
fs.reduce_mixture(MixtureParameters(mix_pr_s, immstate.components))
for fs, mix_pr_s in zip(self.filters, mix_probabilities)
]
return mixed_states
def update(
self,
z: np.ndarray,
immstate: MixtureParameters[MT],
sensor_state: Dict[str, Any] = None,
) -> MixtureParameters[MT]:
"""Update the immstate with z in sensor_state."""
updated_weights = self.update_mode_probabilities(z, immstate, sensor_state=sensor_state)
updated_states = self.mode_matched_update(z, immstate, sensor_state=sensor_state)
updated_immstate = MixtureParameters(updated_weights, updated_states)
return updated_immstate
def update(
self,
z: np.ndarray,
immstate: MixtureParameters[MT],
sensor_state: Dict[str, Any] = None,
) -> MixtureParameters[MT]:
"""Update the immstate with z in sensor_state."""
updated_weights = # TODO
updated_states = # TODO
updated_immstate = MixtureParameters(updated_weights, updated_states)
return updated_immstate
def mix_states( #sverre: step 2: want to obtain a gaussian for every model s_k at time k, that is a best fit description of
#sverre: p(x_k-1 | s_k, Z_1:k-1}
#kjøres for hver mode. Basert på mix_probabilities blir normalfordelingene redusert til én eneste.
self,
immstate: MixtureParameters[MT],
# the mixing probabilities: shape=(M, M)
mix_probabilities: np.ndarray,
) -> List[MT]:
mixed_states = [
fs.reduce_mixture(MixtureParameters(mix_pr_s, immstate.components)) #sverre: immstate is of type MixtureParameters
#sverre: which has the members weights (np.ndarray) and components (sequence[T]).
#components inneholder mean og kovarians til tilstanden
for fs, mix_pr_s in zip(self.filters, mix_probabilities)
#sverre: for every filter the probability for a single model s_k-1 was the true model at time step k-1, given the
#sverre: model s_k is the true model at time step k (gaussian) is reduced with the immstate components
]
return mixed_states
def reduce_mixture(
self, immstate_mixture: MixtureParameters[MixtureParameters[MT]]
) -> MixtureParameters[MT]:
"""Approximate a mixture of immstates as a single immstate"""
# extract probabilities as array
weights = immstate_mixture.weights
component_conditioned_mode_prob = np.array(
[c.weights.ravel() for c in immstate_mixture.components])
# flip conditioning order with Bayes
mode_prob, mode_conditioned_component_prob = None # TODO
# Hint list_a of lists_b to list_b of lists_a: zip(*immstate_mixture.components)
mode_states = None # TODO:
immstate_reduced = MixtureParameters(mode_prob, mode_states)
return immstate_reduced
def reduce_mixture(
self, immstate_mixture: MixtureParameters[MixtureParameters[MT]]
) -> MixtureParameters[MT]:
"""Approximate a mixture of immstates as a single immstate"""
# extract probabilities as array
weights = immstate_mixture.weights
component_conditioned_mode_prob = np.array(
[c.weights.ravel() for c in immstate_mixture.components]
)
# flip conditioning order with Bayes
#components, is this p(X|SK), BUT WE WANT P(SK|X). Is mode_prob p(x)
mode_prob, mode_conditioned_component_prob = discretebayes.discrete_bayes(weights, component_conditioned_mode_prob) # Done
# Hint list_a of lists_b to list_b of lists_a: zip(*immstate_mixture.components)
mode_states = None # TODO:
immstate_reduced = MixtureParameters(mode_prob, mode_states)
return immstate_reduced
def predict(
self,
immstate: MixtureParameters[MT],
# sampling time
Ts: float,
) -> MixtureParameters[MT]:
"""
Predict the immstate Ts time units ahead approximating the mixture step.
Ie. Predict mode probabilities, condition states on predicted mode,
appoximate resulting state distribution as Gaussian for each mode, then predict each mode.
"""
predicted_mode_probability, mixing_probability = \
self.mix_probabilities(immstate, Ts)
mixed_mode_states: List[MT] = \
self.mix_states(immstate, mixing_probability)
predicted_mode_states = \
self.mode_matched_prediction(mixed_mode_states, Ts)
predicted_immstate = MixtureParameters(predicted_mode_probability,
predicted_mode_states)
print_predict = False
if print_predict:
print("\n\npredict:")
print("predicted_mode_probability", predicted_mode_probability,
np.shape(predicted_mode_probability))
print("mixing_probability", mixing_probability,
np.shape(mixing_probability))
print("mixed_mode_states", mixed_mode_states,
np.shape(mixed_mode_states))
print("predicted_mode_states", predicted_mode_states,
np.shape(predicted_mode_states))
print("predicted_immstate", predicted_immstate)
return predicted_immstate
# 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])
p10 = 0.3 # initvalue for mode probabilities
p20 = 0.35
p30 = 0.35
# PI = np.array([[PI11, (1-PI11)/4, 3*(1-PI11)/4], [(1-PI22)/4, PI22, 3*(1-PI22)/4], [(1-PI33)/2, (1-PI33)/2, PI33]]) #alternate transition matrix, more likely to jump into high CV if changing modes
PI = np.array([[PI11, (1 - PI11) / 2, (1 - PI11) / 2],
[(1 - PI22) / 2, PI22, (1 - PI22) / 2],
[(1 - PI33) / 2, (1 - PI33) / 2, PI33]])
assert np.allclose(np.sum(PI, axis=1), 1), "rows of PI must sum to 1"
mean_init = np.array([7100, 3620, 0, 0, 0])
cov_init = np.diag([
40, 40, 10, 10, 0.1
])**2 # np.diag([1000, 1000, 30, 30, 0.1]) ** 2 # THIS WILL NOT BE GOOD
mode_probabilities_init = np.array([p10, p20])
mode_states_init = GaussParams(mean_init, cov_init)
init_imm_state = MixtureParameters(mode_probabilities_init,
[mode_states_init] * 2)
if run_three_models:
mode_probabilities_init = np.array([p10, p20, p30])
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))
ekf_filters = []
ekf_filters.append(ekf.EKF(dynamic_models[0], measurement_model))
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))
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))
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
"""
# extract probabilities as array
## eg. association weights/beta: Pr(a)
weights = immstate_mixture.weights # Pr{a | Z_1:k}
## 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
] # Pr{s | a}
)
# 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)
# 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]
num_modes = len(self.filters)
flipped_mixture_components = []
for (s, prob_a_cond_s) in zip(range(num_modes),
mode_conditioned_component_prob):
components_across_a = [
imm_component.components[s]
for imm_component in immstate_mixture.components
]
flipped_mixture_components.append(
MixtureParameters(prob_a_cond_s, components_across_a))
mode_states: List[GaussParams] = [
self.filters[s].reduce_mixture(mixture_params)
for (s, mixture_params
) in zip(range(num_modes), flipped_mixture_components)
]
immstate_reduced = MixtureParameters(mode_prob, mode_states)
return immstate_reduced
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