Filters

This page describes the base class for designing an UKF (UKF()) and a Jacobian UKF, JUKF(), which is well adapted when the dimension of the state is important. JUKF() infers numerical Jacobian, is relatively less intuitive and gets exactly the same results as UKF(). We finally add a base class for an Extended Kalman Filter (EKF()) that requires analytical Jacobian computation.

UKF

class ukfm.UKF(f, h, phi, phi_inv, Q, R, alpha, state0, P0)

The Unscented Kalman Filter on (parallelizable) Manifolds.

This filter is the implementation described in [BBB19] . It is well adapted to relatively small systems and for understanding the methodology of UKF-M, otherwise see JUKF(). Noise covariance parameters are assumed static for convenience, i.e. \(\mathbf{Q}_n = \mathbf{Q}\), and \(\mathbf{R}_n = \mathbf{R}\).

Parameters

• f – propagation function \(f\).

• h – observation function \(h\).

• phi – retraction \(\boldsymbol{\varphi}\).

• phi_inv – inverse retraction \(\boldsymbol{\varphi}^{-1}\).

• alpha – sigma point parameters. Must be 1D array with 3 values.

Variables

• Q – propagation noise covariance matrix (static) \(\mathbf{Q}\).

• R – observation noise covariance matrix (static) \(\mathbf{R}\).

• state – state \(\boldsymbol{\hat{\chi}}_n\), initialized at state0.

• P – state uncertainty covariance \(\mathbf{P}_n\), initialized at P0.

class WEIGHTS(d, q, alpha)

Sigma point weights.

Weights are computed as:

\[\begin{split}\lambda &= (\alpha^2 - 1) \mathrm{dim}, \\ w_j &= 1/(2(\mathrm{dim} + \lambda)), \\ w_m &= \lambda/(\lambda + \mathrm{dim}), \\ w_0 &= \lambda/(\lambda + \mathrm{dim}) + 3 - \alpha^2,\end{split}\]

where \(\alpha\) is a parameter set between \(10^{-3}\) and \(1\), and \(\mathrm{dim}\) is the dimension of the sigma-points (\(d\) or \(q\)).

This variable contains sigma point weights for propagation (w.r.t. state uncertainty and noise) and for update.

propagation(omega, dt)

UKF propagation step.

\[\begin{split}\boldsymbol{\hat{\chi}}_{n} &\leftarrow \boldsymbol{\hat{\chi}}_{n+1} = f\left(\boldsymbol{\hat{\chi}}_{n}, \boldsymbol{\omega}_{n}, \mathbf{0}\right) \\ \mathbf{P}_{n} &\leftarrow \mathbf{P}_{n+1} \\\end{split}\]

Mean state and covariance are propagated.

Variables

• omega – input \(\boldsymbol{\omega}\).

• dt – integration step \(dt\) (s).

update(y)

UKF update step.

\[\begin{split}\boldsymbol{\hat{\chi}}_{n} &\leftarrow \boldsymbol{\hat{\chi}} _{n}^{+} \\ \mathbf{P}_{n} &\leftarrow \mathbf{P}_{n}^{+} \\\end{split}\]

Variables

y – 1D array (vector) measurement \(\mathbf{y}_n\).

JUKF

class ukfm.JUKF(f, h, phi, Q, alpha, state0, P0, red_phi, red_phi_inv, red_idxs, up_phi, up_idxs, aug_z=None, aug_phi=None, aug_phi_inv=None, aug_idxs=array([0]), aug_q=1)

The Unscented Kalman Filter on (parallelizable) Manifolds, that infers Jacobian.

This filter is an alternative implementation to the method described in [BBB19], with exactly the same results. It spares computational time for systems when only a part of the state is involved in a propagation or update step. It can also be used for state augmentation. Only noise covariance parameter for propagation is assumed static for convenience, i.e. \(\mathbf{Q}_n = \mathbf{Q}\).

Parameters

• f – propagation function \(f\).

• h – observation function \(h\).

• phi – retraction \(\boldsymbol{\varphi}\).

• alpha – sigma point parameters. Must be 1D array with 5 values.

• red_phi – reduced retraction for propagation.

• red_phi_inv – reduced inverse retraction for propagation.

• red_idxs – indices corresponding to the reduced uncertainty.

• up_phi – retraction for update.

• up_idxs – indices corresponding to the state uncertainty for update.

• aug_z – augmentation function \(z\). (optional)

• aug_phi – retraction for augmenting state. (optional)

• aug_phi_inv – inverse retraction for augmenting state. (optional)

• aug_idxs – indices corresponding to the state uncertainty for state augmentation. (optional)

• aug_q – state uncertainty dimension for augmenting state. (optional)

Variables

• Q – propagation noise covariance matrix (static) \(\mathbf{Q}\).

• state – state \(\boldsymbol{\hat{\chi}}_n\), initialized at state0.

• P – state uncertainty covariance \(\mathbf{P}_n\), initialized at P0.

class WEIGHTS(red_d, q, up_d, aug_d, aug_q, alpha)

Sigma point weights.

Weights are computed as:

\[\begin{split}\lambda &= (\alpha^2 - 1) \mathrm{dim}, \\ w_j &= 1/(2(\mathrm{dim} + \lambda)), \\ w_m &= \lambda/(\lambda + \mathrm{dim}), \\ w_0 &= \lambda/(\lambda + \mathrm{dim}) + 3 - \alpha^2,\end{split}\]

where \(\alpha\) is a parameter set between \(10^{-3}\) and \(1\), and \(\mathrm{dim}\) the dimension of the sigma-points.

This variable contains sigma point weights for propagation (w.r.t. state uncertainty and noise), update and state augmentation.

F_num(omega, dt)

Numerical Jacobian computation of \(\mathbf{F}\).

Variables

• omega – input \(\boldsymbol{\omega}\).

• dt – integration step \(dt\) (s).

propagation(omega, dt)

UKF propagation step.

\[\begin{split}\boldsymbol{\hat{\chi}}_{n} &\leftarrow \boldsymbol{\hat{\chi}} _{n+1} = f\left(\boldsymbol{\hat{\chi}}_{n}, \boldsymbol{\omega}_{n}, \mathbf{0}\right) \\ \mathbf{P}_{n} &\leftarrow \mathbf{P}_{n+1} = \mathbf{F} \mathbf{P}_{n} \mathbf{F}^T + \mathbf{G} \mathbf{Q} \mathbf{G}^T \\\end{split}\]

Mean state and covariance are propagated. Covariance is propagated as an EKF, where Jacobian \(\mathbf{F}\) and \(\mathbf{G}\) are numerically inferred.

Variables

• omega – input \(\boldsymbol{\omega}\).

• dt – integration step \(dt\) (s).

state_propagation(omega, dt)

Propagate mean state.

Variables

• omega – input \(\boldsymbol{\omega}\).

• dt – integration step \(dt\) (s).

G_num(omega, dt)

Numerical Jacobian computation of \(\mathbf{G}\).

Variables

• omega – input \(\boldsymbol{\omega}\).

• dt – integration step \(dt\) (s).

cov_propagation()

Covariance propagation.

Variables

• omega – input \(\boldsymbol{\omega}\).

• dt – integration step \(dt\) (s).

update(y, R)

State update, where Jacobian is computed.

Variables

• y – 1D array (vector) measurement \(\mathbf{y}_n\).

• R – measurement covariance \(\mathbf{R}_n\).

H_num(y, idxs, R)

Numerical Jacobian computation of \(\mathbf{H}\).

Variables

• y – 1D array (vector) measurement \(\mathbf{y}_n\).

• idxs – indices corresponding to the state uncertainty for update.

• R – measurement covariance \(\mathbf{R}_n\).

state_update()

State update, once Jacobian is computed.

aug(y, aug_idxs, R)

State augmentation.

Variables

• y – 1D array (vector) measurement \(\mathbf{y}_n\).

• aug_idxs – indices corresponding to the state augmentation uncertainty.

• R – measurement covariance \(\mathbf{R}_n\).

EKF

class ukfm.EKF(model, FG_ana, H_ana, phi, Q, R, state0, P0)

The Extended Kalman Filter on (parallelizable) manifolds.

This is the counterpart of the UKF on (parallelizable) manifolds, where functions for computing Jacobian need to be provided.

Parameters

• model – model that contains propagation function \(f\) and observation function \(h\).

• phi – retraction \(\boldsymbol{\varphi}\).

• FG_ana – function for computing Jacobian \(\mathbf{F}\) and \(\mathbf{G}\) during propagation.

• H_ana – function for computing Jacobian \(\mathbf{H}\) during update.

Variables

• Q – propagation noise covariance matrix (static) \(\mathbf{Q}\).

• R – observation noise covariance matrix (static) \(\mathbf{R}\).

• state – state \(\boldsymbol{\hat{\chi}}_n\), initialized at state0.

• P – state uncertainty covariance \(\mathbf{P}_n\), initialized at P0.

propagation(omega, dt)

EKF propagation step.

\[\begin{split}\boldsymbol{\hat{\chi}}_{n} &\leftarrow \boldsymbol{\hat{\chi}}_{n+1} = f\left(\boldsymbol{\hat{\chi}}_{n}, \boldsymbol{\omega}_{n}, \mathbf{0}\right) \\ \mathbf{P}_{n} &\leftarrow \mathbf{P}_{n+1} = \mathbf{F} \mathbf{P}_{n} \mathbf{F}^T + \mathbf{G} \mathbf{Q} \mathbf{G}^T \\\end{split}\]

Mean state and covariance are propagated.

Variables

• omega – input \(\boldsymbol{\omega}\).

• dt – integration step \(dt\) (s).

update(y)

EKF update step.

\[\begin{split}\boldsymbol{\hat{\chi}}_{n} &\leftarrow \boldsymbol{\hat{\chi}}_{n}^{+} \\ \mathbf{P}_{n} &\leftarrow \mathbf{P}_{n}^{+} \\\end{split}\]

Variables

y – 1D array (vector) measurement \(\mathbf{y}_n\).