gtsam/examples/IEKF_NavstateExample.cpp at develop · Redux-Robotics/gtsam · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file IEKF_NavstateExample.cpp
 * @brief InvariantEKF on NavState (SE_2(3)) with IMU (predict) and GPS (update)
 * @date April 25, 2025
 * @authors Scott Baker, Matt Kielo, Frank Dellaert
 */

#include <gtsam/base/Matrix.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/navigation/InvariantEKF.h>
#include <gtsam/navigation/NavState.h>

#include <iostream>

using namespace std;
using namespace gtsam;

/**
 * @brief Left-invariant dynamics for NavState.
 * @param imu  6×1 vector [a; ω]: linear acceleration and angular velocity.
 * @return     9×1 tangent: [ω; 0₃; a].
 */
Vector9 dynamics(const Vector6& imu) {
  auto a = imu.head<3>();
  auto w = imu.tail<3>();
  Vector9 xi;
  xi << w, Vector3::Zero(), a;
  return xi;
}

/**
 * @brief GPS measurement model: returns position and its Jacobian.
 * @param X    Current state.
 * @param H    Optional 3×9 Jacobian w.r.t. X.
 * @return     3×1 position vector.
 */
Vector3 h_gps(const NavState& X, OptionalJacobian<3, 9> H = {}) {
  return X.position(H);
}

int main() {
  // Initial state & covariances
  NavState X0;  // R=I, v=0, t=0
  Matrix9 P0 = Matrix9::Identity() * 0.1;
  InvariantEKF<NavState> ekf(X0, P0);

  // Noise & timestep
  double dt = 1.0;
  // Continuous-time process noise (scaled by dt inside predict)
  Matrix9 Qc = Matrix9::Identity() * 0.01;
  Matrix3 R = Matrix3::Identity() * 0.5;

  // Two IMU samples [a; ω]
  Vector6 imu1;
  imu1 << 0.1, 0, 0, 0, 0.2, 0;
  Vector6 imu2;
  imu2 << 0, 0.3, 0, 0.4, 0, 0;

  // Two GPS fixes
  Vector3 z1;
  z1 << 0.3, 0, 0;
  Vector3 z2;
  z2 << 0.6, 0, 0;

  cout << "=== Init ===\nX: " << ekf.state() << "\nP: " << ekf.covariance()
    << "\n\n";

  // --- first predict/update ---
  ekf.predict(dynamics(imu1), dt, Qc);
  cout << "--- After predict 1 ---\nX: " << ekf.state()
    << "\nP: " << ekf.covariance() << "\n\n";
  ekf.update(h_gps, z1, R);
  cout << "--- After update 1 ---\nX: " << ekf.state()
    << "\nP: " << ekf.covariance() << "\n\n";

  // --- second predict/update ---
  ekf.predict(dynamics(imu2), dt, Qc);
  cout << "--- After predict 2 ---\nX: " << ekf.state()
    << "\nP: " << ekf.covariance() << "\n\n";
  ekf.update(h_gps, z2, R);
  cout << "--- After update 2 ---\nX: " << ekf.state()
    << "\nP: " << ekf.covariance() << "\n";

  return 0;
}