Skip to content

Commit 8e1e3d2

Browse files
dellaertdellaert
authored
Merge pull request #2263 from borglab/fix/formatting
Formatting/naming in Nav
2 parents 505f12c + 94bf153 commit 8e1e3d2

File tree

11 files changed

+572
-503
lines changed

11 files changed

+572
-503
lines changed

‎gtsam/navigation/InvariantEKF.h‎

Lines changed: 128 additions & 88 deletions
Original file line numberDiff line numberDiff line change
@@ -9,105 +9,145 @@
99
1010
* -------------------------------------------------------------------------- */
1111

12-
/**
13-
* @file InvariantEKF.h
14-
* @brief Left-Invariant Extended Kalman Filter implementation.
15-
*
16-
* This file defines the InvariantEKF class template, inheriting from LieGroupEKF,
17-
* which specifically implements the Left-Invariant EKF formulation. It restricts
18-
* prediction methods to only those based on group composition (state-independent
19-
* motion models), hiding the state-dependent prediction variants from LieGroupEKF.
20-
*
21-
* @date April 24, 2025
22-
* @authors Scott Baker, Matt Kielo, Frank Dellaert
23-
*/
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+
*/
2425

2526
#pragma once
2627

27-
#include <gtsam/navigation/LeftLinearEKF.h> // Include the base class
28-
#include <gtsam/base/Lie.h> // For traits (needed for AdjointMap, Expmap)
29-
28+
#include <gtsam/base/Lie.h> // For traits (needed for AdjointMap, Expmap)
29+
#include <gtsam/navigation/LeftLinearEKF.h> // Include the base class
3030

3131
namespace gtsam {
3232

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+
*/
51+
template <typename G>
52+
class 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+
3363
/**
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 interface
40-
* to only the left-invariant prediction methods:
41-
* 1. Prediction via group composition: `predict(const G& U, const Covariance& Q)`
42-
* 2. Prediction via tangent control vector: `predict(const TangentVector& u, double dt, const Covariance& Q)`
43-
*
44-
* The state-dependent prediction methods from LeftLinearEKF are hidden.
45-
* The update step remains the same as in ManifoldEKF/LeftLinearEKF.
46-
* For details on how static and dynamic dimensions are handled, please refer to
47-
* the `ManifoldEKF` class documentation.
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.
4873
*/
49-
template <typename G>
50-
class InvariantEKF : public LeftLinearEKF<G> {
51-
public:
52-
using Base = LeftLinearEKF<G>; ///< Base class type
53-
using TangentVector = typename Base::TangentVector; ///< Tangent vector type
54-
/// Jacobian for group-specific operations like AdjointMap. Eigen::Matrix<double, Dim, Dim>.
55-
using Jacobian = typename Base::Jacobian;
56-
/// Covariance matrix type. Eigen::Matrix<double, Dim, Dim>.
57-
using Covariance = typename Base::Covariance;
58-
59-
60-
/**
61-
* Constructor: forwards to LeftLinearEKF constructor.
62-
* @param X0 Initial state on Lie group G.
63-
* @param P0 Initial covariance in the tangent space at X0.
64-
*/
65-
InvariantEKF(const G& X0, const Covariance& P0) : Base(X0, P0) {}
66-
67-
// We hide state-dependent predict methods from LeftLinearEKF by only providing the
68-
// invariant predict methods below.
69-
70-
/**
71-
* Predict step via group composition (Left-Invariant):
72-
* X_{k+1} = X_k * U
73-
* P_{k+1} = Ad_{U^{-1}} P_k Ad_{U^{-1}}^T + Q
74-
* where Ad_{U^{-1}} is the Adjoint map of U^{-1}.
75-
*
76-
* @param U Lie group element representing the motion increment.
77-
* @param Q Process noise covariance.
78-
*/
79-
void predict(const G& U, const Covariance& Q) {
80-
this->X_ = traits<G>::Compose(this->X_, U);
74+
static G Dynamics(const G& X, const G& U, OptionalJacobian<Dim, Dim> A = {}) {
75+
if (A) {
8176
const G U_inv = traits<G>::Inverse(U);
82-
const Jacobian A = traits<G>::AdjointMap(U_inv);
83-
// P_ is Covariance. A is Jacobian. Q is Covariance.
84-
// All are Eigen::Matrix<double,Dim,Dim>.
85-
this->P_ = A * this->P_ * A.transpose() + Q;
77+
*A = traits<G>::AdjointMap(U_inv);
8678
}
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.
8784

88-
/**
89-
* Predict step via tangent control vector:
90-
* U = Expmap(u * dt)
91-
* Then calls predict(U, Q).
92-
*
93-
* @param u Tangent space control vector.
94-
* @param dt Time interval.
95-
* @param Q Process noise covariance matrix.
96-
*/
97-
void predict(const TangentVector& u, double dt, const Covariance& Q) {
98-
G U;
99-
if constexpr (std::is_same_v<G, Matrix>) {
100-
// Specialize to Matrix case as its Expmap is not defined.
101-
const Matrix& X = static_cast<const Matrix&>(this->X_);
102-
U.resize(X.rows(), X.cols());
103-
Eigen::Map<Vector>(static_cast<Matrix&>(U).data(), U.size()) = u * dt;
104-
}
105-
else {
106-
U = traits<G>::Expmap(u * dt);
107-
}
108-
predict(U, Q); // Call the group composition predict
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);
109121
}
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+
}
110150

111-
}; // InvariantEKF
151+
}; // InvariantEKF
112152

113-
} // namespace gtsam
153+
} // namespace gtsam

‎gtsam/navigation/LeftLinearEKF.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -70,7 +70,7 @@ class LeftLinearEKF : public LieGroupEKF<G> {
7070
template <class Phi, typename = std::enable_if_t<is_automorphism<Phi>::value>>
7171
static G Dynamics(const G& W, const Phi& phi, const G& X, const G& U,
7272
OptionalJacobian<Dim, Dim> A = {}) {
73-
return W * Dynamics<Phi>(phi, X, U, A); // A is independent of W
73+
return traits<G>::Compose(W, Dynamics<Phi>(phi, X, U, A));
7474
}
7575

7676
/**
@@ -84,7 +84,7 @@ class LeftLinearEKF : public LieGroupEKF<G> {
8484
const G U_inv = traits<G>::Inverse(U);
8585
*A = traits<G>::AdjointMap(U_inv) * phi.dIdentity();
8686
}
87-
return phi(X) * U;
87+
return traits<G>::Compose(phi(X), U);
8888
}
8989

9090
/**

0 commit comments

Comments
 (0)