def mix_states(
self,
immstate: MixtureParameters[MT],
# the mixing probabilities: shape=(M, M)
mix_probabilities: np.ndarray,
) -> List[MT]:
means = []
covs = []
for component in immstate.components:
means.append(component.mean)
covs.append(component.cov)
# now we have a list of all the means and covariances
# to mix it we can use eq. 6.29 and 6.30 from the book
mixed_states = []
for weights in mix_probabilities:
mean_bar, cov_bar = \
mixturereduction.gaussian_mixture_moments(weights, means, covs)
mixed_states.append(GaussParams(mean_bar, cov_bar))
mixed_states = np.array(mixed_states)
print_mixed_states = False
if print_mixed_states:
print("\n\nmixed_states:")
print("mix_probabilities", mix_probabilities)
print("means", means)
print("covs", covs)
print("mixed_states", mixed_states, np.shape(mixed_states))
return mixed_states
def reduce_mixture(
self, ekfstate_mixture: MixtureParameters[GaussParams]
) -> GaussParams:
"""Merge a Gaussian mixture into single mixture"""
w = ekfstate_mixture.weights
x = np.array([c.mean for c in ekfstate_mixture.components], dtype=float)
P = np.array([c.cov for c in ekfstate_mixture.components], dtype=float)
x_reduced, P_reduced = mixturereduction.gaussian_mixture_moments(w, x, P)
return GaussParams(x_reduced, P_reduced)
def mix_states(
self,
immstate: MixtureParameters[MT],
# the mixing probabilities: shape=(M, M)
mix_probabilities: np.ndarray,
) -> List[MT]:
#My comment: Here we are implementing the step 2 of the algorithm, 6.29 and 6.30
means = np.array([component.mean for component in immstate.components])
covs = np.array([component.cov for component in immstate.components])
mixed_states = gaussian_mixture_moments(mix_probabilities, immstate.components[:].mean, immstate.components[:].cov)
mixed_states = []
for i in range(len(means)):
mixed_states.append(gaussian_mixture_moments(mix_probabilities[i], means[i], covs[i]))
mixed_states = np.array(mixed_states)
return mixed_states
def estimate(self, immstate: MixtureParameters[MT]) -> GaussParams:
"""Calculate a state estimate with its covariance from immstate"""
# ! You can assume all the modes have the same reduce and estimate function
# ! and use eg. self.filters[0] functionality
data_reduced = [
gaussian_mixture_moments(immstate.weights, comp.mean, comp.cov)
for comp in immstate.components
]
estimate = self.filters[0].estimate(data_reduced)
return estimate
def estimate(self, immstate: MixtureParameters[MT]) -> GaussParams:
"""Calculate a state estimate with its covariance from immstate"""
# ! You can assume all the modes have the same reduce and estimate function
# ! and use eg. self.filters[0] functionality
#data_reduced = # TODO
means = []
covs = []
for comp in immstate.components:
means.append(comp.mean)
covs.append(comp.cov)
means = np.array(means)
covs = np.array(covs)
mean_reduced, cov_reduced = \
mixturereduction.gaussian_mixture_moments(immstate.weights, means, covs)
estimate = GaussParams(mean_reduced, cov_reduced)
return estimate
from scipy.stats import multivariate_normal
from mixturereduction import gaussian_mixture_moments
# %% setup and show initial
# TODO: fill in values
mus = np.array([0, 2, 4.5]).reshape(3, 1)
sigmas = np.array([1, 1.5, 1.5]).reshape(
3, 1, 1) # note std and not var as in gaussian_mixture_moments
print(sigmas)
w = np.array([1 / 3, 1 / 3, 1 / 3])
#w = np.array(np.ones(shape=(3,1)))
w = w.ravel() / np.sum(w)
assert np.allclose(w.sum(), 1), "weights must sum to one"
totMean, totSigma2 = (elem.squeeze()
for elem in gaussian_mixture_moments(w, mus, sigmas**2))
plotNsigmas = 3
x = totMean + plotNsigmas * np.sqrt(totSigma2) * np.arange(-1, 1 + 1e-10, 5e-2)
fig1, ax1 = plt.subplots(num=1, clear=True)
pdf_comp_vals = np.array([
multivariate_normal.pdf(x, mean=mus[i].item(), cov=sigmas[i].item()**2)
for i in range(len(mus))
])
pdf_mix_vals = np.average(pdf_comp_vals, axis=0, weights=w)
for i in range(len(mus)):
ax1.plot(x, pdf_comp_vals[i], label=f"comp {i}")
ax1.legend()
# %% merge and show combinations
