Left invariant EKF header file, with an example on SE(2) and NavState

examples/LIEKF_NavstateExample.cpp

@@ -0,0 +1,123 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 LIEKF_NavstateExample.cpp
+ * @brief A left invariant Extended Kalman Filter example using the GeneralLIEKF
+ * on NavState using IMU/GPS measurements.
+ *
+ * This example uses the templated GeneralLIEKF class to estimate the state of
+ * an object using IMU/GPS measurements. The prediction stage of the LIEKF uses
+ * a generic dynamics function to predict the state. This simulates a navigation
+ * state of (pose, velocity, position)
+ *
+ * @date Apr 25, 2025
+ * @author Scott Baker
+ * @author Matt Kielo
+ * @author Frank Dellaert
+ */
+#include <gtsam/navigation/NavState.h>
+#include <gtsam/nonlinear/LIEKF.h>
+
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+
+// Define a dynamics function.
+// The dynamics function for NavState returns a result vector of
+// size 9 of [angular_velocity, 0, 0, 0, linear_acceleration] as well as
+// a Jacobian of the dynamics function with respect to the state X.
+// Since this is a left invariant EKF, the error dynamics do not rely on the
+// state
+Vector9 dynamics(const NavState& X, const Vector6& imu,
+                 OptionalJacobian<9, 9> H = {}) {
+  const auto a = imu.head<3>();
+  const auto w = imu.tail<3>();
+  Vector9 result;
+  result << w, Z_3x1, a;
+  if (H) {
+    *H = Matrix::Zero(9, 9);
+  }
+  return result;
+}
+
+// define a GPS measurement processor. The GPS measurement processor returns
+// the expected measurement h(x) = translation of X with a Jacobian H used in
+// the update stage of the LIEKF.
+Vector3 h_gps(const NavState& X, OptionalJacobian<3, 9> H = {}) {
+  if (H) *H << Z_3x3, Z_3x3, X.R();
+  return X.t();
+}
+
+int main() {
+  // Initialize the filter's state, covariance, and time interval values.
+  NavState X0;
+  Matrix9 P0 = Matrix9::Identity() * 0.1;
+  double dt = 1.0;
+
+  // Create the measurement function h_func that wraps h_gps
+  GeneralLIEKF<NavState, Vector3, 6>::MeasurementFunction h_func =
+      [](const NavState& X, OptionalJacobian<3, 9> H) { return h_gps(X, H); };
+
+  // Create the dynamics function dynamics_func
+  GeneralLIEKF<NavState, Vector3, 6>::Dynamics dynamics_func = dynamics;
+
+  // Create the filter with the initial state, covariance, and dynamics and
+  // measurement functions.
+  GeneralLIEKF<NavState, Vector3, 6> ekf(X0, P0, dynamics_func, h_func);
+
+  // Create the process covariance and measurement covariance matrices Q and R.
+  Matrix9 Q = Matrix9::Identity() * 0.01;
+  Matrix3 R = Matrix3::Identity() * 0.5;
+
+  // Create the IMU measurements of the form (linear_acceleration,
+  // angular_velocity)
+  Vector6 imu1, imu2;
+  imu1 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+  imu2 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+
+  // Create the GPS measurements of the form (px, py, pz)
+  Vector3 z1, z2;
+  z1 << 0.0, 0.0, 0.0;
+  z2 << 0.0, 0.0, 0.0;
+
+  // Run the predict and update stages, and print their results.
+  cout << "\nInitialization:\n";
+  cout << "X0: " << ekf.state() << endl;
+  cout << "P0: " << ekf.covariance() << endl;
+
+  // First prediction stage
+  ekf.predict(imu1, dt, Q);
+  cout << "\nFirst Prediction:\n";
+  cout << "X: " << ekf.state() << endl;
+  cout << "P: " << ekf.covariance() << endl;
+
+  // First update stage
+  ekf.update(z1, R);
+  cout << "\nFirst Update:\n";
+  cout << "X: " << ekf.state() << endl;
+  cout << "P: " << ekf.covariance() << endl;
+
+  // Second prediction stage
+  ekf.predict(imu2, dt, Q);
+  cout << "\nSecond Prediction:\n";
+  cout << "X: " << ekf.state() << endl;
+  cout << "P: " << ekf.covariance() << endl;
+
+  // Second update stage
+  ekf.update(z2, R);
+  cout << "\nSecond Update:\n";
+  cout << "X: " << ekf.state() << endl;
+  cout << "P: " << ekf.covariance() << endl;
+
+  return 0;
+}

examples/LIEKF_SE2_SimpleGPSExample.cpp

@@ -0,0 +1,124 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 LIEKF_SE2_SimpleGPSExample.cpp
+ * @brief A left invariant Extended Kalman Filter example using a Lie group
+ * odometry as the prediction stage on SE(2) and
+ *
+ * This example uses the templated LIEKF class to estimate the state of
+ * an object using odometry / GPS measurements The prediction stage of the LIEKF
+ * uses a Lie Group element to propagate the stage in a discrete LIEKF. For most
+ * cases, U = exp(u^ * dt) if u is a control vector that is constant over the
+ * interval dt. However, if u is not constant over dt, other approaches are
+ * needed to find the value of U. This approach simply takes a Lie group element
+ * U, which can be found in various different ways.
+ *
+ * This data was compared to a left invariant EKF on SE(2) using identical
+ * measurements and noise from the source of the InEKF plugin
+ * https://inekf.readthedocs.io/en/latest/ Based on the paper "An Introduction
+ * to the Invariant Extended Kalman Filter" by Easton R. Potokar, Randal W.
+ * Beard, and Joshua G. Mangelson
+ *
+ * @date Apr 25, 2025
+ * @author Scott Baker
+ * @author Matt Kielo
+ * @author Frank Dellaert
+ */
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/nonlinear/LIEKF.h>
+
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+
+// Create a 2D GPS measurement function that returns the predicted measurement h
+// and Jacobian H. The predicted measurement h is the translation of the state
+// X.
+Vector2 h_gps(const Pose2& X, OptionalJacobian<2, 3> H = {}) {
+  return X.translation(H);
+}
+// Define a LIEKF class that uses the Pose2 Lie group as the state and the
+// Vector2 measurement type.
+int main() {
+  // // Initialize the filter's state, covariance, and time interval values.
+  Pose2 X0(0.0, 0.0, 0.0);
+  Matrix3 P0 = Matrix3::Identity() * 0.1;
+  double dt = 1.0;
+
+  // Create the function measurement_function that wraps h_gps.
+  std::function<Vector2(const Pose2&, gtsam::OptionalJacobian<2, 3>)>
+      measurement_function = h_gps;
+  // Create the filter with the initial state, covariance, and measurement
+  // function.
+  LIEKF<Pose2, Vector2> ekf(X0, P0, measurement_function);
+
+  // Define the process covariance and measurement covariance matrices Q and R.
+  Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal();
+  Matrix2 R = (Vector2(0.01, 0.01)).asDiagonal();
+
+  // Define odometry movements.
+  // U1: Move 1 unit in X, 1 unit in Y, 0.5 radians in theta.
+  // U2: Move 1 unit in X, 1 unit in Y, 0 radians in theta.
+  Pose2 U1(1.0, 1.0, 0.5), U2(1.0, 1.0, 0.0);
+
+  // Create GPS measurements.
+  // z1: Measure robot at (1, 0)
+  // z2: Measure robot at (1, 1)
+  Vector2 z1, z2;
+  z1 << 1.0, 0.0;
+  z2 << 1.0, 1.0;
+
+  // Define a transformation matrix to convert the covariance into (theta, x, y)
+  // form. The paper and data mentioned above uses a (theta, x, y) notation,
+  // whereas GTSAM uses (x, y, theta). The transformation matrix is used to
+  // directly compare results of the covariance matrix.
+  Matrix3 TransformP;
+  TransformP << 0, 0, 1, 1, 0, 0, 0, 1, 0;
+
+  // Propagating/updating the filter
+  // Initial state and covariance
+  cout << "\nInitialization:\n";
+  cout << "X0: " << ekf.state() << endl;
+  cout << "P0: " << TransformP * ekf.covariance() * TransformP.transpose()
+       << endl;
+
+  // First prediction stage
+  ekf.predict(U1, Q);
+  cout << "\nFirst Prediction:\n";
+  cout << "X: " << ekf.state() << endl;
+  cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose()
+       << endl;
+
+  // First update stage
+  ekf.update(z1, R);
+  cout << "\nFirst Update:\n";
+  cout << "X: " << ekf.state() << endl;
+  cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose()
+       << endl;
+
+  // Second prediction stage
+  ekf.predict(U2, Q);
+  cout << "\nSecond Prediction:\n";
+  cout << "X: " << ekf.state() << endl;
+  cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose()
+       << endl;
+
+  // Second update stage
+  ekf.update(z2, R);
+  cout << "\nSecond Update:\n";
+  cout << "X: " << ekf.state() << endl;
+  cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose()
+       << endl;
+
+  return 0;
+}