Add liekf example for se2_localization

examples/CMakeLists.txt

@@ -4,6 +4,7 @@ add_executable(se2_average se2_average.cpp)
 add_executable(se2_interpolation se2_interpolation.cpp)
 add_executable(se2_decasteljau se2_DeCasteljau.cpp)
 add_executable(se2_localization se2_localization.cpp)
+add_executable(se2_localization_liekf se2_localization_liekf.cpp)
 add_executable(se2_localization_ukfm se2_localization_ukfm.cpp)
 add_executable(se2_sam se2_sam.cpp)
 
@@ -23,6 +24,7 @@ set(CXX_11_EXAMPLE_TARGETS
   se2_decasteljau
   se2_average
   se2_localization
+  se2_localization_liekf
   se2_sam
 
   se2_localization_ukfm

examples/se2_localization_liekf.cpp

@@ -0,0 +1,282 @@
+/**
+ * \file se2_localization_liekf.cpp
+ *
+ *  Created on: Jan 20, 2021
+ *     \author: artivis
+ *
+ *  Modified on: Aug 30, 2024
+ *     \author: chris
+ *
+ *  ---------------------------------------------------------
+ *  This file is:
+ *  (c) 2021 artivis
+ *
+ *  This file is part of `manif`, a C++ template-only library
+ *  for Lie theory targeted at estimation for robotics.
+ *  Manif is:
+ *  (c) 2021 artivis
+ *  ---------------------------------------------------------
+ *
+ *  ---------------------------------------------------------
+ *  Demonstration example:
+ *
+ *  2D Robot localization based on position measurements (GPS-like)
+ *  using the (Left) Invariant Extended Kalman Filter method.
+ *
+ *  See se2_localization.cpp for the right invariant equivalent
+ *  ---------------------------------------------------------
+ *
+ *  This demo corresponds to the application in chapter 4.3 (left invariant)
+ *  in the thesis
+ *  'Non-linear state error based extended Kalman filters with applications to navigation' A. Barrau.
+ *
+ *  It is adapted after the sample problem presented in chapter 4.1 (left invariant)
+ *  in the thesis
+ *  'Practical Considerations and Extensions of the Invariant Extended Kalman Filtering Framework' J. Arsenault
+ *
+ *  Finally, it is ported from an example (a matlab code plus a problem
+ *  formulation/explanation document) by Faraaz Ahmed and James Richard
+ *  Forbes, McGill University.
+ *
+ *  The following is an abstract of the content of the paper.
+ *  Please consult the paper for better reference.
+ *
+ *
+ *  We consider a robot in the plane.
+ *  The robot receives control actions in the form of axial
+ *  and angular velocities, and is able to measure its position
+ *  using a GPS for instance.
+ *
+ *  The robot pose X is in SE(2) and the beacon positions b_k in R^2,
+ *
+ *          | cos th  -sin th   x |
+ *      X = | sin th   cos th   y |  // position and orientation
+ *          |   0        0      1 |
+ *
+ *  The control signal u is a twist in se(2) comprising longitudinal
+ *  velocity v and angular velocity w, with no lateral velocity
+ *  component, integrated over the sampling time dt.
+ *
+ *      u = (v*dt, 0, w*dt)
+ *
+ *  The control is corrupted by additive Gaussian noise u_noise,
+ *  with covariance
+ *
+ *    Q = diagonal(sigma_v^2, sigma_s^2, sigma_w^2).
+ *
+ *  This noise accounts for possible lateral slippage u_s
+ *  through a non-zero value of sigma_s,
+ *
+ *  At the arrival of a control u, the robot pose is updated
+ *  with X <-- X * Exp(u) = X + u.
+ *
+ *  GPS measurements are put in Cartesian form for simplicity.
+ *  Their noise n is zero mean Gaussian, and is specified
+ *  with a covariances matrix R.
+ *  We notice the rigid motion action y = h(X) = X * [0 0 1]^T + v
+ *
+ *      y_k = (x, y)       // robot coordinates
+ *
+ *  We define the pose to estimate as X in SE(2).
+ *  The estimation error dx and its covariance P are expressed
+ *  in the global space at epsilon.
+ *
+ *  All these variables are summarized again as follows
+ *
+ *    X   : robot pose, SE(2)
+ *    u   : robot control, (v*dt ; 0 ; w*dt) in se(2)
+ *    Q   : control perturbation covariance
+ *    y   : robot position measurement in global frame, R^2
+ *    R   : covariance of the measurement noise
+ *
+ *  The motion and measurement models are
+ *
+ *    X_(t+1) = f(X_t, u) = X_t * Exp ( w )     // motion equation
+ *    y_k     = h(X, b_k) = X * [0 0 1]^T       // measurement equation
+ *
+ *  The algorithm below comprises first a simulator to
+ *  produce measurements, then uses these measurements
+ *  to estimate the state, using a Lie-based
+ *  Left Invariant Kalman filter.
+ *
+ *  This file has plain code with only one main() function.
+ *  There are no function calls other than those involving `manif`.
+ *
+ *  Printing simulated state and estimated state together
+ *  with an unfiltered state (i.e. without Kalman corrections)
+ *  allows for evaluating the quality of the estimates.
+ */
+
+#include "manif/SE2.h"
+
+#include <vector>
+
+#include <iostream>
+#include <iomanip>
+
+using std::cout;
+using std::endl;
+
+using namespace Eigen;
+
+typedef Array<double, 2, 1> Array2d;
+typedef Array<double, 3, 1> Array3d;
+
+int main()
+{
+    std::srand((unsigned int) time(0));
+
+    // START CONFIGURATION
+    //
+    //
+    const Matrix3d I = Matrix3d::Identity();
+
+    // Define the robot pose element and its covariance
+    manif::SE2d X, X_simulation, X_unfiltered;
+    Matrix3d    P, P_L;
+
+    X_simulation.setIdentity();
+    X.setIdentity();
+    X_unfiltered.setIdentity();
+    P.setZero();
+
+    // Define a control vector and its noise and covariance
+    manif::SE2Tangentd  u_simu, u_est, u_unfilt;
+    Vector3d            u_nom, u_noisy, u_noise;
+    Array3d             u_sigmas;
+    Matrix3d            U;
+
+    u_nom    << 0.1, 0.0, 0.05;
+    u_sigmas << 0.1, 0.1, 0.1;
+    U        = (u_sigmas * u_sigmas).matrix().asDiagonal();
+
+    // Declare the Jacobians of the motion wrt robot and control
+    manif::SE2d::Jacobian J_x, J_u, AdX, AdXinv;
+
+    // Define the gps measurements in R^2
+    Vector2d    y, y_noise;
+    Array2d     y_sigmas;
+    Matrix2d    R;
+
+    y_sigmas << 0.1, 0.1;
+    R        = (y_sigmas * y_sigmas).matrix().asDiagonal();
+
+    // Declare the Jacobian of the measurements wrt the robot pose
+    Matrix<double, 2, 3>    H;      // H = J_e_x
+
+    // Declare some temporaries
+    Vector2d                e, z;   // expectation, innovation
+    Matrix2d                E, Z;   // covariances of the above
+    Matrix<double, 3, 2>    K;      // Kalman gain
+    manif::SE2Tangentd      dx;     // optimal update step, or error-state
+
+    //
+    //
+    // CONFIGURATION DONE
+
+
+
+    // DEBUG
+    cout << std::fixed   << std::setprecision(3) << std::showpos << endl;
+    cout << "X STATE     :    X      Y    THETA" << endl;
+    cout << "----------------------------------" << endl;
+    cout << "X initial   : " << X_simulation.log().coeffs().transpose() << endl;
+    cout << "----------------------------------" << endl;
+    // END DEBUG
+
+
+
+
+    // START TEMPORAL LOOP
+    //
+    //
+
+    // Make 10 steps. Measure up to three landmarks each time.
+    for (int t = 0; t < 10; t++)
+    {
+        //// I. Simulation ###############################################################################
+
+        /// simulate noise
+        u_noise = u_sigmas * Array3d::Random();             // control noise
+        u_noisy = u_nom + u_noise;                          // noisy control
+
+        u_simu   = u_nom;
+        u_est    = u_noisy;
+        u_unfilt = u_noisy;
+
+        /// first we move - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+        X_simulation = X_simulation + u_simu;               // overloaded X.rplus(u) = X * exp(u)
+
+        /// then we receive noisy gps measurement - - - - - - - - - - - - - - - -
+        y_noise = y_sigmas * Array2d::Random();             // simulate measurement noise
+
+        y = X_simulation.translation();                     // position measurement, before adding noise
+        y = y + y_noise;                                    // position measurement, noisy
+
+
+
+
+        //// II. Estimation ###############################################################################
+
+        /// First we move - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+
+        X = X.plus(u_est, J_x, J_u);                        // X * exp(u), with Jacobians
+
+        P = J_x * P * J_x.transpose() + J_u * U * J_u.transpose();
+
+
+        /// Then we correct using the gps position - - - - - - - - - - - - - - -
+
+        // transform covariance from right to left invariant
+        AdX = X.adj();
+        AdXinv = X.inverse().adj();
+        P_L = AdX * P * AdX.transpose();                    // from eq. (54) in the paper
+
+        // expectation
+        e = X.translation();                                // e = t, for X = (R,t).
+        H.topLeftCorner<2, 2>() = Matrix2d::Identity();
+        H.topRightCorner<2, 1>() = manif::skew(1.0) * X.translation();
+        E = H * P_L * H.transpose();
+
+        // innovation
+        z = y - e;
+        Z = E + R;
+
+        // Kalman gain
+        K = P_L * H.transpose() * Z.inverse();
+
+        // Correction step
+        dx = K * z;
+
+        // Update
+        X = X.lplus(dx);
+        P = AdXinv * ((I - K * H) * P_L) * AdXinv.transpose();
+
+
+
+
+        //// III. Unfiltered ##############################################################################
+
+        // move also an unfiltered version for comparison purposes
+        X_unfiltered = X_unfiltered + u_unfilt;
+
+
+
+
+        //// IV. Results ##############################################################################
+
+        // DEBUG
+        cout << "X simulated : " << X_simulation.log().coeffs().transpose() << endl;
+        cout << "X estimated : " << X.log().coeffs().transpose() << endl;
+        cout << "X unfilterd : " << X_unfiltered.log().coeffs().transpose() << endl;
+        cout << "----------------------------------" << endl;
+        // END DEBUG
+
+    }
+
+    //
+    //
+    // END OF TEMPORAL LOOP. DONE.
+
+    return 0;
+}