Skip to content

Commit

Permalink
added comments, cleaned up utils/mixture
Browse files Browse the repository at this point in the history
  • Loading branch information
syedshabbirahmed committed Jun 27, 2024
1 parent c1d9160 commit 08b700a
Show file tree
Hide file tree
Showing 2 changed files with 21 additions and 13 deletions.
26 changes: 21 additions & 5 deletions navlie/filters.py
Original file line number Diff line number Diff line change
Expand Up @@ -982,13 +982,26 @@ def correct(

class GaussianSumFilter:
"""
On-manifold Gaussian Sum Filter.
On-manifold Gaussian Sum Filter (GSF).
References for the GSF:
D. Alspach and H. Sorenson, "Nonlinear Bayesian estimation using
Gaussian sum approximations," in IEEE Transactions on Automatic
Control, vol. 17, no. 4, pp. 439-448, August 1972.
The GSF involves Gaussian mixtures. Reference for mixing Gaussians on
manifolds:
J. Ćesić, I. Marković and I. Petrović, "Mixture Reduction on Matrix Lie
Groups," in IEEE Signal Processing Letters, vol. 24, no. 11, pp.
1719-1723, Nov. 2017, doi: 10.1109/LSP.2017.2723765.
"""

__slots__ = [
"process_model",
"reject_outliers",
"ekf",
"_ekf",
]

def __init__(
Expand All @@ -1004,7 +1017,7 @@ def __init__(
reject_outliers : bool, optional
whether to apply the NIS test to measurements, by default False
"""
self.ekf = ExtendedKalmanFilter(process_model, reject_outliers)
self._ekf = ExtendedKalmanFilter(process_model, reject_outliers)

def predict(
self,
Expand Down Expand Up @@ -1039,7 +1052,7 @@ def predict(

x_check = []
for i in range(n_modes):
x_check.append(self.ekf.predict(x.model_states[i], u, dt))
x_check.append(self._ekf.predict(x.model_states[i], u, dt))
return MixtureState(x_check, x.model_probabilities)

def correct(
Expand Down Expand Up @@ -1071,7 +1084,7 @@ def correct(
x_hat = []
weights_hat = np.zeros(n_modes)
for i in range(n_modes):
x, details_dict = self.ekf.correct(x_check.model_states[i], y, u,
x, details_dict = self._ekf.correct(x_check.model_states[i], y, u,
output_details=True)
x_hat.append(x)
z = details_dict["z"]
Expand Down Expand Up @@ -1159,6 +1172,9 @@ def run_filter(
return results_list


# TODO. The IMM seems to have an issue when the user accidently modifies the
# provided state in the process model.

def run_imm_filter(
filter: InteractingModelFilter,
x0: State,
Expand Down
8 changes: 0 additions & 8 deletions navlie/utils/mixture.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,17 +6,9 @@

from navlie.types import (
State,
Input,
Measurement,
StateWithCovariance,
)
import numpy as np
from navlie.lib import MixtureState


# TODO. The IMM seems to have an issue when the user accidently modifies the
# provided state in the process model.



def gaussian_mixing_vectorspace(
Expand Down

0 comments on commit 08b700a

Please sign in to comment.