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 IEKF_NavstateExample.cpp
14 * @brief InvariantEKF on NavState (SE_2(3)) with IMU (predict) and GPS (update)
15 * @date April 25, 2025
16 * @authors Scott Baker, Matt Kielo, Frank Dellaert
17 */
18
19#include <gtsam/base/Matrix.h>
20#include <gtsam/base/VectorSpace.h>
21#include <gtsam/base/OptionalJacobian.h>
22#include <gtsam/navigation/InvariantEKF.h>
23#include <gtsam/navigation/NavState.h>
24
25#include <iostream>
26
27using namespace std;
28using namespace gtsam;
29
30/**
31 * @brief Left-invariant dynamics for NavState.
32 * @param imu 6×1 vector [a; ω]: linear acceleration and angular velocity.
33 * @return 9×1 tangent: [ω; 0₃; a].
34 */
35Vector9 dynamics(const Vector6& imu) {
36 auto a = imu.head<3>();
37 auto w = imu.tail<3>();
38 Vector9 xi;
39 xi << w, Vector3::Zero(), a;
40 return xi;
41}
42
43/**
44 * @brief GPS measurement model: returns position and its Jacobian.
45 * @param X Current state.
46 * @param H Optional 3×9 Jacobian w.r.t. X.
47 * @return 3×1 position vector.
48 */
49Vector3 h_gps(const NavState& X, OptionalJacobian<3, 9> H = {}) {
50 return X.position(H);
51}
52
53int main() {
54 // Initial state & covariances
55 NavState X0; // R=I, v=0, t=0
56 Matrix9 P0 = Matrix9::Identity() * 0.1;
57 InvariantEKF<NavState> ekf(X0, P0);
58
59 // Noise & timestep
60 double dt = 1.0;
61 Matrix9 Q = Matrix9::Identity() * 0.01;
62 Matrix3 R = Matrix3::Identity() * 0.5;
63
64 // Two IMU samples [a; ω]
65 Vector6 imu1;
66 imu1 << 0.1, 0, 0, 0, 0.2, 0;
67 Vector6 imu2;
68 imu2 << 0, 0.3, 0, 0.4, 0, 0;
69
70 // Two GPS fixes
71 Vector3 z1;
72 z1 << 0.3, 0, 0;
73 Vector3 z2;
74 z2 << 0.6, 0, 0;
75
76 cout << "=== Init ===\nX: " << ekf.state() << "\nP: " << ekf.covariance()
77 << "\n\n";
78
79 // --- first predict/update ---
80 ekf.predict(u: dynamics(imu: imu1), dt, Q);
81 cout << "--- After predict 1 ---\nX: " << ekf.state()
82 << "\nP: " << ekf.covariance() << "\n\n";
83 ekf.update(h&: h_gps, z: z1, R);
84 cout << "--- After update 1 ---\nX: " << ekf.state()
85 << "\nP: " << ekf.covariance() << "\n\n";
86
87 // --- second predict/update ---
88 ekf.predict(u: dynamics(imu: imu2), dt, Q);
89 cout << "--- After predict 2 ---\nX: " << ekf.state()
90 << "\nP: " << ekf.covariance() << "\n\n";
91 ekf.update(h&: h_gps, z: z2, R);
92 cout << "--- After update 2 ---\nX: " << ekf.state()
93 << "\nP: " << ekf.covariance() << "\n";
94
95 return 0;
96}
97