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