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 GEKF_Rot3Example.cpp
14 * @brief Left‐Invariant EKF on SO(3) with state‐dependent pitch/roll control
15 * and a single magnetometer update.
16 * @date April 25, 2025
17 * @authors Scott Baker, Matt Kielo, Frank Dellaert
18 */
19
20#include <gtsam/base/Matrix.h>
21#include <gtsam/base/OptionalJacobian.h>
22#include <gtsam/geometry/Rot3.h>
23#include <gtsam/navigation/LieGroupEKF.h>
24
25#include <iostream>
26
27using namespace std;
28using namespace gtsam;
29
30// --- 1) Closed‐loop dynamics f(X): xi = –k·[φx,φy,0], H = ∂xi/∂φ·Dφ ---
31static constexpr double k = 0.5;
32Vector3 dynamicsSO3(const Rot3& X, OptionalJacobian<3, 3> H = {}) {
33 // φ = Logmap(R), Dφ = ∂φ/∂δR
34 Matrix3 D_phi;
35 Vector3 phi = Rot3::Logmap(R: X, H: D_phi);
36 // zero out yaw
37 phi[2] = 0.0;
38 D_phi.row(i: 2).setZero();
39
40 if (H) *H = -k * D_phi; // ∂(–kφ)/∂δR
41 return -k * phi; // xi ∈ 𝔰𝔬(3)
42}
43
44// --- 2) Magnetometer model: z = R⁻¹ m, H = –[z]_× ---
45static const Vector3 m_world(0, 0, -1);
46Vector3 h_mag(const Rot3& X, OptionalJacobian<3, 3> H = {}) {
47 Vector3 z = X.inverse().rotate(p: m_world);
48 if (H) *H = -skewSymmetric(w: z);
49 return z;
50}
51
52int main() {
53 // Initial estimate (identity) and covariance
54 const Rot3 R0 = Rot3::RzRyRx(x: 0.1, y: -0.2, z: 0.3);
55 const Matrix3 P0 = Matrix3::Identity() * 0.1;
56 LieGroupEKF<Rot3> ekf(R0, P0);
57
58 // Timestep, process noise, measurement noise
59 double dt = 0.1;
60 Matrix3 Q = Matrix3::Identity() * 0.01;
61 Matrix3 Rm = Matrix3::Identity() * 0.05;
62
63 cout << "=== Init ===\nR:\n"
64 << ekf.state().matrix() << "\nP:\n"
65 << ekf.covariance() << "\n\n";
66
67 // Predict using state‐dependent f
68 ekf.predict(f&: dynamicsSO3, dt, Q);
69 cout << "--- After predict ---\nR:\n" << ekf.state().matrix() << "\n\n";
70
71 // Magnetometer measurement = body‐frame reading of m_world
72 Vector3 z = h_mag(X: R0);
73 ekf.update(h&: h_mag, z, R: Rm);
74 cout << "--- After update ---\nR:\n" << ekf.state().matrix() << "\n";
75
76 return 0;
77}
78