Add files via upload

A Left Invariant IEKF on SE(2).

examples/SE2LIEKF_NoFactors.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 SE2LIEKF_NoFactors.cpp
+ *
+ * A simple left invariant EKF operating in SE(2) using an odometry and GPS measurements.
+ * No factors are used here.
+ * 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 April 1, 2025
+ * @author Scott Baker
+ */
+
+
+
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Point2.h>
+#include <Eigen/Dense>
+#include <gtsam/base/Matrix.h>
+
+using namespace std;
+using namespace gtsam;
+
+#define PI    3.14159265358
+
+// Create an adjoint class
+// Function for the prediction stage.
+// Note that U is the exponential map of some control vector (u*dt)
+  void predict(Pose2& X, Matrix& P,  Vector3& u, Matrix& Q)
+      {
+    //Pose2 U = Pose2::Expmap(u*dt);
+    Pose2 U(u(0), u(1), u(2));
+    Matrix Adj = U.inverse().AdjointMap();
+    X = X.compose(U);
+    P = Adj * P * Adj.transpose() + Q;
+    }
+
+  // Update stage that uses GPS measurements in a left invariant update way.
+  void update(Pose2& X, Matrix& P, Point2& z, Matrix& H, Matrix& R)
+        {
+    // find residual R^T * (z - zhat)
+    Point2 zhat = X.translation();
+    Point2 e_world = z-zhat;
+    Point2 residual = X.rotation().unrotate(e_world);
+    Matrix S = H*P*H.transpose() + R;
+    Matrix K = P*H.transpose()*S.inverse();
+    Vector3 delta = K*residual;
+    X = X * Pose2::Expmap(delta);
+    P = (Matrix3::Identity() - K*H) * P;
+    }
+
+
+
+int main() {
+
+  // Define movements and measurements
+  const double dt = 1.0;
+
+  Vector3 u1(1.0, 1.0, 0.5), u2(1.0,1.0,0.0); // odometry
+  Point2 z1(1.0, 0.0), z2(1.0, 1.0); // gps
+
+
+
+  // Set up noise models (uses simple GPS)
+
+  // Odometry Process
+  Vector3 pModel(0.05, 0.05, 0.001);    // covariance of the odometry process (rad)
+  Matrix Q = pModel.asDiagonal();       // Q covariance matrix
+
+  // GPS process
+  Vector2 rModel(0.01, 0.01);
+  Matrix R = rModel.asDiagonal();
+  Matrix H(2,3);
+  H << 1,0,0,
+      0,1,0;
+
+  // A matrix that transforms the P matrix into a form of [theta, x, y] for comparison to InEKF's structure.
+  Matrix3 TransformP;
+  TransformP << 0, 0, 1,
+      1,0,0,
+      0,1,0;
+
+// Initialize
+  Pose2 X(0, 0, 0);
+  Matrix P = Matrix3::Identity()*0.1;
+
+  cout << "Initial X\n" << X << endl;
+  cout << "Initial P\n" << TransformP * P * TransformP.transpose() << endl;
+
+ // First Prediction
+  predict(X, P, u1, dt, Q);
+  cout << "Predicted X1\n" << X << endl;
+  cout << "Predicted P1\n" << TransformP * P * TransformP.transpose() << endl;
+
+  // First Update
+  update(X, P, z1, H, R);
+  cout << "Updated X1\n" << X << endl;
+  cout << "Updated P1\n" << TransformP * P * TransformP.transpose() << endl;
+
+  // Second Prediction
+  predict(X, P, u2, dt, Q);
+  cout << "Predicted X2\n" << X << endl;
+  cout << "Predicted P2\n" << TransformP * P * TransformP.transpose() << endl;
+
+  // Second Update
+  update(X, P, z2, H, R);
+  cout << "Updated X2\n" << X << endl;
+  cout << "Updated P2\n" << TransformP * P * TransformP.transpose() << endl;
+
+  return 0;
+  }
+