1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file InvariantEKF.h
14 * @brief Left-Invariant Extended Kalman Filter implementation.
15 *
16 * This file defines the InvariantEKF class template, inheriting from
17 * LieGroupEKF, which specifically implements the Left-Invariant EKF
18 * formulation. It restricts prediction methods to only those based on group
19 * composition (state-independent motion models), hiding the state-dependent
20 * prediction variants from LieGroupEKF.
21 *
22 * @date April 24, 2025
23 * @authors Scott Baker, Matt Kielo, Frank Dellaert
24 */
25
26#pragma once
27
28#include <gtsam/base/Lie.h> // For traits (needed for AdjointMap, Expmap)
29#include <gtsam/navigation/LeftLinearEKF.h> // Include the base class
30
31namespace gtsam {
32
33/**
34 * @class InvariantEKF
35 * @brief Left-Invariant Extended Kalman Filter on a Lie group G.
36 *
37 * @tparam G Lie group type (must satisfy LieGroup concept).
38 *
39 * This filter inherits from LeftLinearEKF but restricts the prediction
40 * interface to only the left-invariant prediction methods:
41 * 1. Prediction via group composition: `predict(const G& U, const Covariance&
42 * Q)`
43 * 2. Prediction via tangent control vector: `predict(const TangentVector& u,
44 * double dt, const Covariance& Q)`
45 *
46 * The state-dependent prediction methods from LeftLinearEKF are hidden.
47 * The update step remains the same as in ManifoldEKF/LeftLinearEKF.
48 * For details on how static and dynamic dimensions are handled, please refer to
49 * the `ManifoldEKF` class documentation.
50 */
51template <typename G>
52class InvariantEKF : public LeftLinearEKF<G> {
53 public:
54 using Base = LeftLinearEKF<G>; ///< Base class type
55 static constexpr int Dim = Base::Dim; ///< Compile-time dimension of G.
56 using TangentVector = typename Base::TangentVector; ///< Tangent vector type
57 /// Jacobian for group-specific operations like AdjointMap.
58 /// Eigen::Matrix<double, Dim, Dim>.
59 using Jacobian = typename Base::Jacobian;
60 /// Covariance matrix type. Eigen::Matrix<double, Dim, Dim>.
61 using Covariance = typename Base::Covariance;
62
63 /**
64 * Constructor: forwards to LeftLinearEKF constructor.
65 * @param X0 Initial state on Lie group G.
66 * @param P0 Initial covariance in the tangent space at X0.
67 */
68 InvariantEKF(const G& X0, const Covariance& P0) : Base(X0, P0) {}
69
70 /**
71 * Dynamics with W=I.
72 * Returns φ(X) · U, and optional Jacobian A = Ad_{U^{-1}} Φ, Φ := dφ|_e.
73 */
74 static G Dynamics(const G& X, const G& U, OptionalJacobian<Dim, Dim> A = {}) {
75 if (A) {
76 const G U_inv = traits<G>::Inverse(U);
77 *A = traits<G>::AdjointMap(U_inv);
78 }
79 return traits<G>::Compose(X, U);
80 }
81
82 // We hide state-dependent predict methods from LeftLinearEKF by only
83 // providing the invariant predict methods below.
84
85 /**
86 * Predict step via group composition (Left-Invariant):
87 * X_{k+1} = X_k * U
88 * P_{k+1} = Ad_{U^{-1}} P_k Ad_{U^{-1}}^T + Q
89 * where Ad_{U^{-1}} is the Adjoint map of U^{-1}.
90 *
91 * @param U Lie group element representing the motion increment.
92 * @param Q Process noise covariance.
93 */
94 void predict(const G& U, const Covariance& Q) {
95 this->X_ = traits<G>::Compose(this->X_, U);
96 const G U_inv = traits<G>::Inverse(U);
97 const Jacobian A = traits<G>::AdjointMap(U_inv);
98 // P_ is Covariance. A is Jacobian. Q is Covariance.
99 // All are Eigen::Matrix<double,Dim,Dim>.
100 this->P_ = A * this->P_ * A.transpose() + Q;
101 }
102
103 /**
104 * Predict step via tangent control vector:
105 * U = Expmap(u * dt)
106 * Then calls predict(U, Q).
107 *
108 * @param u Tangent space control vector.
109 * @param dt Time interval.
110 * @param Q Process noise covariance matrix.
111 */
112 void predict(const TangentVector& u, double dt, const Covariance& Q) {
113 G U;
114 if constexpr (std::is_same_v<G, Matrix>) {
115 // Specialize to Matrix case as its Expmap is not defined.
116 const Matrix& X = static_cast<const Matrix&>(this->X_);
117 U.resize(X.rows(), X.cols());
118 Eigen::Map<Vector>(static_cast<Matrix&>(U).data(), U.size()) = u * dt;
119 } else {
120 U = traits<G>::Expmap(u * dt);
121 }
122 predict(U, Q); // Call the group composition predict
123 }
124
125 /**
126 * General dynamics.
127 * Returns W · X · U, and optional Jacobian A = Ad_{U^{-1}}
128 */
129 static G Dynamics(const G& W, const G& X, const G& U,
130 OptionalJacobian<Dim, Dim> A = {}) {
131 return traits<G>::Compose(W, Dynamics(X, U, A)); // A is independent of W
132 }
133
134 /**
135 * Predict step via left and right group composition (Left-Invariant):
136 * X_{k+1} = W * X_k * U
137 * P_{k+1} = Ad_{U^{-1}} P_k Ad_{U^{-1}}^T + Q
138 * where Ad_{U^{-1}} is the Adjoint map of U^{-1}.
139 *
140 * @param W Lie group element representing the motion increment in world
141 * frame.
142 * @param U Lie group element representing the motion increment in body frame.
143 * @param Q Process noise covariance.
144 */
145 void predict(const G& W, const G& U, const Covariance& Q) {
146 predict(U, Q); // First apply U
147 // Then apply W on the left
148 this->X_ = traits<G>::Compose(W, this->X_);
149 }
150
151}; // InvariantEKF
152
153} // namespace gtsam