|
| void | reset (const G &g, const CovT &P) |
| | Reset the state of the EKF. More...
|
| |
| G | estimate () const |
| | Access filter state estimate. More...
|
| |
| CovT | covariance () const |
| | Access filter covariance. More...
|
| |
| template<typename F , typename QDer > |
| void | predict (F &&f, const Eigen::MatrixBase< QDer > &Q, Scalar< G > tau, std::optional< Scalar< G > > dt={}) |
| | Propagate EKF through dynamics \( \mathrm{d}^r x_t = f(t, x) \) with covariance \(Q\) over a time interval \( [0, \tau] \). More...
|
| |
| template<typename F , typename RDev , Manifold Y = std::invoke_result_t<F, G>> |
| void | update (F &&h, const Y &y, const Eigen::MatrixBase< RDev > &R) |
| | Update EKF with a measurement \(y = h(x) + w\) where \(w \sim \mathcal N(0, R)\). More...
|
| |
template<LieGroup G, diff::Type DiffType = diff::Type::Default, template< typename... > typename Stpr = boost::numeric::odeint::euler>
requires (Dof<G> > 0)
class smooth::feedback::EKF< G, DiffType, Stpr >
Extended Kalman filter on Lie groups.
The primary methods are predict() and update().
- predict(): propagates filter state through a dynamical model.
- update(): Bayesian update of filter state from a measurement.
Use this class for information fusion (without dynamics) by solely using update().
- Template Parameters
-
| G | smooth::LieGroup type. |
| DiffType | smooth::diff::Type method for calculating derivatives. |
| Stpr | boost::numeric::odeint templated stepper type (euler / runge_kutta4 / ...). Defaults to euler. |
Definition at line 32 of file ekf.hpp.
template<LieGroup G, diff::Type DiffType = diff::Type::Default, template< typename... > typename Stpr = boost::numeric::odeint::euler>
template<typename F , typename QDer >
| void smooth::feedback::EKF< G, DiffType, Stpr >::predict |
( |
F && |
f, |
|
|
const Eigen::MatrixBase< QDer > & |
Q, |
|
|
Scalar< G > |
tau, |
|
|
std::optional< Scalar< G > > |
dt = {} |
|
) |
| |
|
inline |
Propagate EKF through dynamics \( \mathrm{d}^r x_t = f(t, x) \) with covariance \(Q\) over a time interval \( [0, \tau] \).
- Parameters
-
| f | right-hand side \( f : \mathbb{R} \times \mathbb{G} \rightarrow \mathbb{R}^{\dim
\mathfrak{g}} \) of the dynamics. The time type must be the scalar type of G. |
| Q | process covariance (size \( \dim \mathfrak{g} \times \dim \mathfrak{g} \)) |
| tau | amount of time to propagate |
| dt | maximal ODE solver step size (defaults to tau, i.e. one step) |
- Note
- The time \( t \) argument of \( f(t, x) \) ranges over the interval \(t \in [0,
\tau] \).
-
The covariance \( Q \) is infinitesimal, i.e. its SI unit is \( S^2/T \) where \(S\) is the unit of state and \(T\) is the unit of time.
-
Only the upper triangular part of Q is used.
Definition at line 80 of file ekf.hpp.
template<LieGroup G, diff::Type DiffType = diff::Type::Default, template< typename... > typename Stpr = boost::numeric::odeint::euler>
template<typename F , typename RDev , Manifold Y = std::invoke_result_t<F, G>>
| void smooth::feedback::EKF< G, DiffType, Stpr >::update |
( |
F && |
h, |
|
|
const Y & |
y, |
|
|
const Eigen::MatrixBase< RDev > & |
R |
|
) |
| |
|
inline |
Update EKF with a measurement \(y = h(x) + w\) where \(w \sim \mathcal N(0, R)\).
- Parameters
-
| h | measurement function \( h : \mathbb{G} \rightarrow \mathbb{Y} \) |
| y | measurement value \( y \in \mathbb{Y} \) |
| R | measurement covariance (size \( \dim \mathbb{Y} \times \dim \mathbb{Y} \)) |
- Note
- The function h must be differentiable using the desired method.
-
Only the upper triangular part of \(R\) is used
Definition at line 117 of file ekf.hpp.
