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_SE2Example.cpp
14 * @brief A left invariant Extended Kalman Filter example using a Lie group
15 * odometry as the prediction stage on SE(2) and
16 *
17 * This example uses the templated InvariantEKF class to estimate the state of
18 * an object using odometry / GPS measurements The prediction stage of the
19 * InvariantEKF uses a Lie Group element to propagate the stage in a discrete
20 * InvariantEKF. For most cases, U = exp(u^ * dt) if u is a control vector that
21 * is constant over the interval dt. However, if u is not constant over dt,
22 * other approaches are needed to find the value of U. This approach simply
23 * takes a Lie group element U, which can be found in various different ways.
24 *
25 * This data was compared to a left invariant EKF on SE(2) using identical
26 * measurements and noise from the source of the InEKF plugin
27 * https://inekf.readthedocs.io/en/latest/ Based on the paper "An Introduction
28 * to the Invariant Extended Kalman Filter" by Easton R. Potokar, Randal W.
29 * Beard, and Joshua G. Mangelson
30 *
31 * @date Apr 25, 2025
32 * @authors Scott Baker, Matt Kielo, Frank Dellaert
33 */
34#include <gtsam/geometry/Pose2.h>
35#include <gtsam/navigation/InvariantEKF.h>
36
37#include <iostream>
38
39using namespace std;
40using namespace gtsam;
41
42// Create a 2D GPS measurement function that returns the predicted measurement h
43// and Jacobian H. The predicted measurement h is the translation of the state
44// X.
45Vector2 h_gps(const Pose2& X, OptionalJacobian<2, 3> H = {}) {
46 return X.translation(Hself: H);
47}
48
49// Define a InvariantEKF class that uses the Pose2 Lie group as the state and
50// the Vector2 measurement type.
51int main() {
52 // // Initialize the filter's state, covariance, and time interval values.
53 Pose2 X0(0.0, 0.0, 0.0);
54 Matrix3 P0 = Matrix3::Identity() * 0.1;
55
56 // Create the filter with the initial state, covariance, and measurement
57 // function.
58 InvariantEKF<Pose2> ekf(X0, P0);
59
60 // Define the process covariance and measurement covariance matrices Q and R.
61 Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal();
62 Matrix2 R = I_2x2 * 0.01;
63
64 // Define odometry movements.
65 // U1: Move 1 unit in X, 1 unit in Y, 0.5 radians in theta.
66 // U2: Move 1 unit in X, 1 unit in Y, 0 radians in theta.
67 Pose2 U1(1.0, 1.0, 0.5), U2(1.0, 1.0, 0.0);
68
69 // Create GPS measurements.
70 // z1: Measure robot at (1, 0)
71 // z2: Measure robot at (1, 1)
72 Vector2 z1, z2;
73 z1 << 1.0, 0.0;
74 z2 << 1.0, 1.0;
75
76 // Define a transformation matrix to convert the covariance into (theta, x, y)
77 // form. The paper and data mentioned above uses a (theta, x, y) notation,
78 // whereas GTSAM uses (x, y, theta). The transformation matrix is used to
79 // directly compare results of the covariance matrix.
80 Matrix3 TransformP;
81 TransformP << 0, 0, 1, 1, 0, 0, 0, 1, 0;
82
83 // Propagating/updating the filter
84 // Initial state and covariance
85 cout << "\nInitialization:\n";
86 cout << "X0: " << ekf.state() << endl;
87 cout << "P0: " << TransformP * ekf.covariance() * TransformP.transpose()
88 << endl;
89
90 // First prediction stage
91 ekf.predict(U: U1, Q);
92 cout << "\nFirst Prediction:\n";
93 cout << "X: " << ekf.state() << endl;
94 cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose()
95 << endl;
96
97 // First update stage
98 ekf.update(h&: h_gps, z: z1, R);
99 cout << "\nFirst Update:\n";
100 cout << "X: " << ekf.state() << endl;
101 cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose()
102 << endl;
103
104 // Second prediction stage
105 ekf.predict(U: U2, Q);
106 cout << "\nSecond Prediction:\n";
107 cout << "X: " << ekf.state() << endl;
108 cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose()
109 << endl;
110
111 // Second update stage
112 ekf.update(h&: h_gps, z: z2, R);
113 cout << "\nSecond Update:\n";
114 cout << "X: " << ekf.state() << endl;
115 cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose()
116 << endl;
117
118 return 0;
119}