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 NavStateImuExample.cpp
14 * @brief Run the NavStateImuEKF on an orbiting (constant twist) scenario.
15 * Uses ScenarioRunner to generate corrupted IMU, de-biases with true
16 * biases, feeds the EKF, and compares to Scenario ground truth.
17 *
18 * @date August 2025
19 * @authors You
20 */
21
22#include <gtsam/base/Matrix.h>
23#include <gtsam/geometry/Cal3_S2.h>
24#include <gtsam/geometry/PinholeCamera.h>
25#include <gtsam/geometry/Pose3.h>
26#include <gtsam/geometry/Rot3.h>
27#include <gtsam/navigation/NavState.h>
28#include <gtsam/navigation/NavStateImuEKF.h>
29#include <gtsam/navigation/PreintegrationParams.h>
30#include <gtsam/navigation/Scenario.h>
31#include <gtsam/navigation/ScenarioRunner.h>
32
33#include <cmath>
34#include <iomanip>
35#include <iostream>
36
37using namespace std;
38using namespace gtsam;
39
40static constexpr double kGravity = 9.81;
41
42int main(int argc, char* argv[]) {
43 // --- Scenario: camera orbiting a point ---
44 const double radius = 30.0;
45 const double angular_velocity = M_PI; // rad/sec
46 const Vector3 w_b(0, 0, -angular_velocity);
47 const Vector3 v_n(radius * angular_velocity, 0, 0);
48 ConstantTwistScenario scenario(w_b, v_n);
49
50 // --- Preintegration/IMU parameters ---
51 auto params = PreintegrationParams::MakeSharedU(g: kGravity);
52 params->setAccelerometerCovariance(I_3x3 * 0.1);
53 params->setGyroscopeCovariance(I_3x3 * 0.1);
54 params->setIntegrationCovariance(I_3x3 * 0.1);
55 params->setUse2ndOrderCoriolis(false);
56 params->setOmegaCoriolis(Vector3::Zero());
57
58 // True biases used to corrupt measurements in ScenarioRunner
59 imuBias::ConstantBias trueBias(Vector3(0.01, -0.005, 0.02), // gyro bias
60 Vector3(0.05, 0.03, -0.02)); // accel bias
61
62 // --- Measurement generator ---
63 const double dt = 1.0 / 180.0; // 1 degree per step
64 ScenarioRunner runner(scenario, params, dt, trueBias);
65
66 // --- Initialize EKF with ground truth
67 const double t0 = 0.0;
68 NavState X0 = scenario.navState(t: t0);
69 Matrix9 P0 = I_9x9 * 1e-2; // modest initial uncertainty
70 NavStateImuEKF ekf(X0, P0, params);
71
72 // --- Run for N steps and compare predictions ---
73 const size_t N = 90;
74 cout << fixed << setprecision(3);
75 cout << "step,t(s),rot_err_deg,pos_err,vel_err\n";
76 double t = t0;
77 for (size_t i = 0; i < N; ++i) {
78 // Measurements from runner, *not* corrupted by noise
79 const Vector3 measuredOmega = runner.actualAngularVelocity(t);
80 const Vector3 measuredAcc = runner.actualSpecificForce(t);
81
82 // De-bias using the true (known) biases before feeding EKF
83 const Vector3 omega = measuredOmega - trueBias.gyroscope();
84 const Vector3 acc = measuredAcc - trueBias.accelerometer();
85
86 // Predict one step
87 ekf.predict(omega_b: omega, f_b: acc, dt);
88
89 // Ground truth at next time
90 t += dt;
91 const NavState gt = scenario.navState(t);
92
93 // Print ground truth and EKF state
94 // cout << "Ground Truth: " << gt << "\n";
95 // cout << "EKF State: " << ekf.state() << "\n";
96
97 // Errors
98 const Rot3 dR = gt.attitude().between(g: ekf.state().attitude());
99 const Vector3 rpy = dR.rpy();
100 const double rot_err_deg =
101 (Vector3(rpy.cwiseAbs()) * (180.0 / M_PI)).norm();
102 const double pos_err = (gt.position() - ekf.state().position()).norm();
103 const double vel_err = (gt.velocity() - ekf.state().velocity()).norm();
104
105 cout << i + 1 << ", error: " << t << "," << rot_err_deg << "," << pos_err
106 << "," << vel_err << "\n";
107 }
108
109 return 0;
110}
111