Loading...
Searching...
No Matches
gtsam::LeftLinearEKF< G > Class Template Reference
Detailed Description
template<typename G>
class gtsam::LeftLinearEKF< G >
class gtsam::LeftLinearEKF< G >
EKF on a Lie group with a general left–linear prediction model.
Discrete step: x⁺ = W · φ(x) · U, with W,U ∈ G and φ ∈ Aut(G). For left-invariant error, the state-independent linearization is A = Ad_{U^{-1}} · Φ where Φ := dφ|_e. The left factor W cancels in A and does not appear there.
Inheritance diagram for gtsam::LeftLinearEKF< G >:
Public Member Functions | |
| LeftLinearEKF (const G &X0, const Covariance &P0) | |
| template<class Phi, typename = std::enable_if_t<is_automorphism<Phi>::value>> | |
| void | predict (const G &W, const Phi &phi, const G &U, const Covariance &Q) |
| General left–linear prediction, updates filter state as follows: X⁺ = W · φ(X) · U P⁺ = A P Aᵀ + Q with A = Ad_{U^{-1}} Φ, Φ := dφ|_e. | |
| template<class Phi, typename = std::enable_if_t<is_automorphism<Phi>::value>> | |
| void | predict (const Phi &phi, const G &U, const Covariance &Q) |
| Special case of predict with W=I, updates filter state as follows: Update: X⁺ = φ(X) · U Covariance: P⁺ = A P Aᵀ + Q with A = Ad_{U^{-1}} Φ, Φ := dφ|_e. | |
| Public Member Functions inherited from gtsam::LieGroupEKF< G > | |
| LieGroupEKF (const G &X0, const Covariance &P0) | |
| Constructor: initialize with state and covariance. | |
| template<size_t K = 1> | |
| Jacobian | transitionMatrix (const TangentVector &xi, const Jacobian &Df, double dt, const G &U, const Jacobian &Dexp) const |
| Compute the discrete-time transition matrix Φ corresponding to a continuous-time linearization (Df) over time dt. | |
| template<size_t K = 1, typename Dynamics, typename = enable_if_dynamics<Dynamics>> | |
| G | predictMean (Dynamics &&f, double dt, OptionalJacobian< Dim, Dim > Phi={}) const |
| Predict mean and Jacobian Phi with state-dependent dynamics: xi = f(X_k, Df) (tangent vector dynamics and Jacobian Df) U = Expmap(xi * dt, Dexp) (motion increment U and Expmap Jacobian Dexp) X_{k+1} = X_k * U (Predict next state via compose) Phi = Ad_{U^{-1}} + Dexp * Df * dt (K=1 first-order Jacobian) expm((Df - ad(xi))dt) (K>1 matrix exponential discretization). | |
| template<size_t K = 1, typename Dynamics, typename = enable_if_dynamics<Dynamics>> | |
| void | predict (Dynamics &&f, double dt, const Covariance &Q) |
| Predict step with state-dependent dynamics: Uses predictMean to compute X_{k+1} and Phi, then updates covariance. | |
| template<size_t K = 1, typename Control, typename Dynamics, typename = enable_if_full_dynamics<Control, Dynamics>> | |
| G | predictMean (Dynamics &&f, const Control &u, double dt, OptionalJacobian< Dim, Dim > Phi={}) const |
| Predict mean and Jacobian A with state and control input dynamics: Wraps the dynamics function and calls the state-only predictMean. | |
| template<size_t K = 1, typename Control, typename Dynamics, typename = enable_if_full_dynamics<Control, Dynamics>> | |
| void | predict (Dynamics &&f, const Control &u, double dt, const Covariance &Q) |
| Predict step with state and control input dynamics: Wraps the dynamics function and calls the state-only predict. | |
| void | predictWithCompose (const G &U, const Jacobian &J_UX, const Covariance &Q) |
| Predict using a precomputed group increment U and its Jacobian J_UX. | |
| void | predict (const G &X_next, const Jacobian &F, const Covariance &Q) |
| Expose base class predict method, predict(const M& X_next, const Jacobian& F, const Covariance& Q). | |
| void | update (const Measurement &prediction, const Eigen::Matrix< double, traits< Measurement >::dimension, Dim > &H, const Measurement &z, const Eigen::Matrix< double, traits< Measurement >::dimension, traits< Measurement >::dimension > &R, bool performReset=true) |
| Update overloads follow ManifoldEKF. | |
| void | update (MeasurementFunction &&h, const Measurement &z, const Eigen::Matrix< double, traits< Measurement >::dimension, traits< Measurement >::dimension > &R, bool performReset=true) |
| Update overloads follow ManifoldEKF. | |
| Public Member Functions inherited from gtsam::ManifoldEKF< G > | |
| ManifoldEKF (const G &X0, const Covariance &P0) | |
| Constructor: initialize with state and covariance. | |
| const G & | state () const |
| const Covariance & | covariance () const |
| size_t | dimension () const |
| void | predict (const G &X_next, const Jacobian &F, const Covariance &Q) |
| Basic predict step: Updates state and covariance given the predicted next state and the state transition Jacobian F. | |
| auto | KalmanGain (const HMatrix &H, const RMatrix &R) const |
| Kalman gain K = P H^T S^-1. | |
| void | JosephUpdate (const GainMatrix &K, const HMatrix &H, const RMatrix &R) |
| Joseph-form covariance update in the current tangent space using a precomputed gain. | |
| void | update (const Measurement &prediction, const Eigen::Matrix< double, traits< Measurement >::dimension, Dim > &H, const Measurement &z, const Eigen::Matrix< double, traits< Measurement >::dimension, traits< Measurement >::dimension > &R, bool performReset=true) |
| Measurement update: Corrects the state and covariance using a pre-calculated predicted measurement and its Jacobian. | |
| void | update (MeasurementFunction &&h, const Measurement &z, const Eigen::Matrix< double, traits< Measurement >::dimension, traits< Measurement >::dimension > &R, bool performReset=true) |
| Measurement update: Corrects the state and covariance using a measurement model function. | |
| void | updateWithVector (const gtsam::Vector &prediction, const Matrix &H, const gtsam::Vector &z, const Matrix &R, bool performReset=true) |
| Convenience bridge for wrappers: vector measurement update calling update<Vector>. | |
| void | reset (const TangentVector &eta) |
| Reset step: retract the state by a tangent perturbation and, if available, transport the covariance from the old tangent space to the new tangent space. | |
Static Public Member Functions | |
| template<class Phi, typename = std::enable_if_t<is_automorphism<Phi>::value>> | |
| static G | Dynamics (const G &W, const Phi &phi, const G &X, const G &U, OptionalJacobian< Dim, Dim > A={}) |
| General left–linear dynamics. | |
| template<class Phi, typename = std::enable_if_t<is_automorphism<Phi>::value>> | |
| static G | Dynamics (const Phi &phi, const G &X, const G &U, OptionalJacobian< Dim, Dim > A={}) |
| Left–linear dynamics with W=I. | |
Static Public Attributes | |
| static constexpr int | Dim = Base::Dim |
| Compile-time dimension of G. | |
| Static Public Attributes inherited from gtsam::LieGroupEKF< G > | |
| static constexpr int | Dim = Base::Dim |
| Compile-time dimension of G. | |
| Static Public Attributes inherited from gtsam::ManifoldEKF< G > | |
| static constexpr int | Dim |
| Compile-time dimension of the manifold M. | |
Public Types | |
| using | Base = LieGroupEKF<G> |
| using | TangentVector = typename Base::TangentVector |
| using | Jacobian = typename Base::Jacobian |
| using | Covariance = typename Base::Covariance |
| Public Types inherited from gtsam::LieGroupEKF< G > | |
| using | This = LieGroupEKF<G> |
| using | Base = ManifoldEKF<G> |
| Base class type. | |
| using | Jacobian = typename Base::Jacobian |
| Dim x Dim. | |
| using | Covariance = typename Base::Covariance |
| Dim x Dim. | |
| using | TangentVector = typename Base::TangentVector |
| Tangent vector type. | |
| Public Types inherited from gtsam::ManifoldEKF< G > | |
| using | TangentVector |
| Tangent vector type for the manifold M. | |
| using | Covariance |
| Covariance matrix type (P, Q). | |
| using | Jacobian |
| State transition Jacobian type (F). | |
Classes | |
| struct | is_automorphism |
| SFINAE template to check if a type satisfies the automorphism concept. More... | |
Additional Inherited Members | |
| Protected Member Functions inherited from gtsam::ManifoldEKF< G > | |
| void | validateInputs (const gtsam::Vector &prediction, const Matrix &H, const gtsam::Vector &z, const Matrix &R) |
| Validate inputs to update. | |
| Static Protected Member Functions inherited from gtsam::ManifoldEKF< G > | |
| static bool | isMatrixOfSize (const MatrixType &matrix, size_t rows, size_t cols) |
| Check whether a matrix has the expected runtime dimensions. | |
| Protected Attributes inherited from gtsam::ManifoldEKF< G > | |
| G | X_ |
| Manifold state estimate. | |
| Covariance | P_ |
| Covariance (Eigen::Matrix<double, Dim, Dim>). | |
| Jacobian | I_ |
| Identity matrix sized to the state dimension. | |
| size_t | n_ |
| Runtime tangent space dimension of M. | |
Member Function Documentation
◆ Dynamics() [1/2]
template<typename G>
template<class Phi, typename = std::enable_if_t<is_automorphism<Phi>::value>>
|
inlinestatic |
General left–linear dynamics.
Returns W · φ(X) · U, and optional Jacobian A = Ad_{U^{-1}} Φ, Φ := dφ|_e.
◆ Dynamics() [2/2]
template<typename G>
template<class Phi, typename = std::enable_if_t<is_automorphism<Phi>::value>>
|
inlinestatic |
Left–linear dynamics with W=I.
Returns φ(X) · U, and optional Jacobian A = Ad_{U^{-1}} Φ, Φ := dφ|_e.
The documentation for this class was generated from the following file:
- /tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/navigation/LeftLinearEKF.h
