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 Gal3ImuEKF.h
14+ * @brief (Invariant) Extended Kalman Filter for IMU-driven Gal3
15+ * We use an invariant Kalman Filter on the Gal3 Lie group to propagate from one
16+ * state to another. X_(k+1) = W*X*U where X(k) ∈ Gal3 follows format of [R, v,
17+ * p; 0, 1, dt; 0, 0, 1]
18+ *
19+ * W is the gravity matrix that transforms body to world frame where
20+ * W ∈ Gal3= [I_3, g * dt, -0.5*g*dt^2
21+ * 0, 1, -dt
22+ * 0, 0, 1]
23+ *
24+ * U ∈ Gal3 is the control matrix where
25+ * U = [dR, f_b*dt, f_b*0.5*dt^2
26+ * 0, 1, dt
27+ * 0, 0, 1]
28+ * @date September 2025
29+ * @authors Scott Baker
30+ */
31+
32+ #pragma once
33+
34+ #include < gtsam/geometry/Gal3.h>
35+ #include < gtsam/navigation/InvariantEKF.h> // Include the base class
36+ #include < gtsam/navigation/PreintegrationParams.h>
37+
38+ namespace gtsam {
39+
40+ // / Specialized EKF for IMU-driven on Gal3
41+ class GTSAM_EXPORT Gal3ImuEKF : public InvariantEKF<Gal3> {
42+ public:
43+ using Base = InvariantEKF<Gal3>;
44+ using TangentVector = typename Base::TangentVector; // Vector10
45+ using Jacobian = typename Base::Jacobian; // 10x10
46+ using Covariance = typename Base::Covariance; // 10x10
47+
48+ // / The Gal3 EKF has three modes of operation
49+ enum Mode {
50+ NO_TIME, // /< Do not track time, state remains in NavState sub-group
51+ TRACK_TIME_NO_COVARIANCE, // /< Track time, but not its covariance (default)
52+ TRACK_TIME_WITH_COVARIANCE, // /< Track time and its covariance.
53+ };
54+
55+ /* *
56+ * Construct with initial state/covariance and preintegration params (for
57+ * gravity and IMU covariances)
58+ * @param X0 Initial Gal3.
59+ * @param P0 Initial covariance in tangent space at X0.
60+ * @param params Preintegration parameters providing gravity and options.
61+ */
62+ Gal3ImuEKF (const Gal3& X0, const Covariance& P0,
63+ const std::shared_ptr<PreintegrationParams>& params,
64+ Mode mode = TRACK_TIME_NO_COVARIANCE);
65+
66+ // / Calculate gravity-only left composition, world-frame increments
67+ // / p = +1/2 g dt^2, v = g dt, t = 0
68+ static Gal3 Gravity (const Vector3& g_n, double dt) {
69+ return {Rot3 (), g_n * (0.5 * dt * dt), g_n * dt, 0.0 };
70+ }
71+
72+ // / Calculate W: gravity with correction to neutralize time change,
73+ // / Using this W(t_k) together with Imu() yields the exact dynamics update,
74+ // / but t stays 0, and hence we stay within NavState sub-group at all times.
75+ static Gal3 TimeZeroingGravity (const Vector3& g_n, double dt) {
76+ return {Rot3 (), -g_n * (0.5 * dt * dt), g_n * dt, -dt};
77+ }
78+
79+ // / Calculate W: position-compensated gravity (left composition) that enables
80+ // / tracking absolute time in-state. Using this W(t_k) together with Imu()
81+ // / yields the exact dynamics update with additionally t_{k+1} = t_k + dt.
82+ static Gal3 CompensatedGravity (const Vector3& g_n, double dt, double t_k) {
83+ const Point3 pW (-t_k * g_n * dt - g_n * (0.5 * dt * dt));
84+ const Vector3 vW = g_n * dt;
85+ return {Rot3 (), pW, vW, 0.0 };
86+ }
87+
88+ // / Calculate U from raw IMU (no gravity): body-frame increments
89+ static Gal3 Imu (const Vector3& omega_b, const Vector3& f_b, double dt) {
90+ Gal3::TangentVector xi;
91+ xi << omega_b, f_b, Z_3x1, 1.0 ;
92+ return Gal3::Expmap (xi * dt);
93+ }
94+
95+ /* *
96+ * @brief Compute the dynamics of the system.
97+ *
98+ * This function computes the next state of the system based on the current
99+ * state, gravity, body angular velocity, body specific force, and time step.
100+ * The dynamics are defined as:
101+ * X_{k+1} = f(X_k; g, omega_b, f_b, dt)
102+ * = W(g, dt) \phi_dt(X_k) U(omega_b, f_b, dt)
103+ * where W, \phi, and U are the gravity, (autonomous) position update, and
104+ * IMU increment functions, respectively.
105+ *
106+ * @param g_n Gravity vector in the navigation frame.
107+ * @param X Current Gal3.
108+ * @param omega_b Body angular velocity measurement (rad/s).
109+ * @param f_b Body specific force measurement (m/s^2).
110+ * @param dt Time step in seconds.
111+ * @param A Optional Jacobian of the dynamics with respect to the state.
112+ * @return The next Gal3 after applying the dynamics.
113+ */
114+ static Gal3 Dynamics (const Vector3& g_n, const Gal3& X,
115+ const Vector3& omega_b, const Vector3& f_b, double dt,
116+ Mode mode = TRACK_TIME_WITH_COVARIANCE,
117+ OptionalJacobian<10 , 10 > A = {});
118+
119+ /* *
120+ * @brief Predict the next state using gyro and accelerometer measurements.
121+ *
122+ * This method updates the state of the system based on the provided body
123+ * angular velocity (omega_b) and body specific force (f_b)
124+ * measurements over a given time step.
125+ *
126+ * @param omega_b Body angular velocity measurement (rad/s).
127+ * @param f_b Body specific force measurement (m/s^2).
128+ * @param dt Time step in seconds.
129+ */
130+ void predict (const Vector3& omega_b, const Vector3& f_b, double dt);
131+
132+ // / Accessors
133+ const std::shared_ptr<PreintegrationParams>&
params ()
const ;
134+ const Vector3& gravity () const ;
135+ const Covariance& processNoise () const ;
136+
137+ private:
138+ std::shared_ptr<PreintegrationParams> params_;
139+ Mode mode_{TRACK_TIME_NO_COVARIANCE};
140+ Covariance Q_ = Covariance::Zero();
141+ };
142+
143+ } // namespace gtsam
0 commit comments