From 6dc0d0c05c1668689accff5ae7b0da8fa260467f Mon Sep 17 00:00:00 2001 From: chris7462 Date: Sat, 31 Aug 2024 20:03:09 -0400 Subject: [PATCH 1/6] Add liekf example for se2_localization --- examples/CMakeLists.txt | 2 + examples/se2_localization_liekf.cpp | 282 ++++++++++++++++++++++++++++ 2 files changed, 284 insertions(+) create mode 100644 examples/se2_localization_liekf.cpp diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index abcff5a9..5548221d 100644 --- a/examples/CMakeLists.txt +++ b/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 diff --git a/examples/se2_localization_liekf.cpp b/examples/se2_localization_liekf.cpp new file mode 100644 index 00000000..816db88c --- /dev/null +++ b/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 GPS measurement y_k is 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 + +#include +#include + +using std::cout; +using std::endl; + +using namespace Eigen; + +typedef Array Array2d; +typedef Array 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 H; // H = J_e_x + + // Declare some temporaries + Vector2d e, z; // expectation, innovation + Matrix2d E, Z; // covariances of the above + Matrix 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 one GPS position 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; +} From 1864eda4abc3c6f98de6d4225c44fb419ec3b7f4 Mon Sep 17 00:00:00 2001 From: chris7462 Date: Tue, 3 Sep 2024 09:57:55 -0400 Subject: [PATCH 2/6] Address requested changes for MR --- examples/se2_localization_liekf.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/examples/se2_localization_liekf.cpp b/examples/se2_localization_liekf.cpp index 816db88c..f2e94dea 100644 --- a/examples/se2_localization_liekf.cpp +++ b/examples/se2_localization_liekf.cpp @@ -38,7 +38,8 @@ * formulation/explanation document) by Faraaz Ahmed and James Richard * Forbes, McGill University. * - * The following is an abstract of the content of the paper. + * The following is an abstract of the content of the paper + * 'A micro Lie theory for state estimation in robotics' J. Solà, J. Deray, D.Atchuthan. * Please consult the paper for better reference. * * @@ -250,7 +251,10 @@ int main() // Update X = X.lplus(dx); - P = AdXinv * ((I - K * H) * P_L) * AdXinv.transpose(); + P_L = (I - K * H) * P_L; + + // transform covariance from left to right invariant + P = AdXinv * P_L * AdXinv.transpose(); From b2be76a135ed50a7aa03b413478df21b91a2a1e7 Mon Sep 17 00:00:00 2001 From: chris7462 Date: Tue, 3 Sep 2024 14:55:28 -0400 Subject: [PATCH 3/6] Fix docs --- examples/se2_localization_liekf.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/examples/se2_localization_liekf.cpp b/examples/se2_localization_liekf.cpp index f2e94dea..522c6c2d 100644 --- a/examples/se2_localization_liekf.cpp +++ b/examples/se2_localization_liekf.cpp @@ -28,19 +28,18 @@ * * 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. + * 'Non-linear state error based extended Kalman filters with applications to navigation' A. Barrau. [1] * * 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 + * 'Practical Considerations and Extensions of the Invariant Extended Kalman Filtering Framework' J. Arsenault [2] * - * 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. + * For covariance transformation between global and local frame, and vice versa, please refer to + * Equation (54) in the paper + * 'A Micro Lie Theory for State Estimation in Robotics' by J. Solà, J. Deray, and D. Atchuthan [3]. * - * The following is an abstract of the content of the paper - * 'A micro Lie theory for state estimation in robotics' J. Solà, J. Deray, D.Atchuthan. - * Please consult the paper for better reference. + * The following is an abstract of the content of the papers. + * Please consult papers [1] and [2] for the simulation setup, and [3] for the covariance transformation. * * * We consider a robot in the plane. From 9d1eb07125ebc0eb9f075bc4c3061aa0de5c3eea Mon Sep 17 00:00:00 2001 From: chris7462 Date: Wed, 11 Sep 2024 07:32:17 -0400 Subject: [PATCH 4/6] Fix Jacobian based on the discussion --- examples/se2_localization_liekf.cpp | 27 +++++++++++++++------------ 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/examples/se2_localization_liekf.cpp b/examples/se2_localization_liekf.cpp index 522c6c2d..db138be8 100644 --- a/examples/se2_localization_liekf.cpp +++ b/examples/se2_localization_liekf.cpp @@ -91,7 +91,7 @@ * * The motion and measurement models are * - * X_(t+1) = f(X_t, u) = X_t * Exp ( w ) // motion equation + * X_(t+1) = f(X_t, u) = X_t * Exp ( u ) // motion equation * y_k = h(X, b_k) = X * [0 0 1]^T // measurement equation * * The algorithm below comprises first a simulator to @@ -146,8 +146,8 @@ int main() Array3d u_sigmas; Matrix3d U; - u_nom << 0.1, 0.0, 0.05; - u_sigmas << 0.1, 0.1, 0.1; + u_nom << 0.1, 0.0, 0.1; + u_sigmas << 0.1, 0.1, 0.05; U = (u_sigmas * u_sigmas).matrix().asDiagonal(); // Declare the Jacobians of the motion wrt robot and control @@ -158,7 +158,7 @@ int main() Array2d y_sigmas; Matrix2d R; - y_sigmas << 0.1, 0.1; + y_sigmas << 0.01, 0.01; R = (y_sigmas * y_sigmas).matrix().asDiagonal(); // Declare the Jacobian of the measurements wrt the robot pose @@ -167,6 +167,7 @@ int main() // Declare some temporaries Vector2d e, z; // expectation, innovation Matrix2d E, Z; // covariances of the above + Matrix2d M; Matrix K; // Kalman gain manif::SE2Tangentd dx; // optimal update step, or error-state @@ -192,7 +193,7 @@ int main() // // Make 10 steps. Measure one GPS position each time. - for (int t = 0; t < 10; t++) + for (int t = 0; t < 1000; t++) { //// I. Simulation ############################################################################### @@ -230,17 +231,19 @@ int main() // 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 + P_L = AdXinv * P * AdXinv.transpose(); // 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(); + H.topLeftCorner<2, 2>() = -Matrix2d::Identity(); + H.topRightCorner<2, 1>() = Vector2d::Zero(); E = H * P_L * H.transpose(); // innovation - z = y - e; - Z = E + R; + M = X.inverse().rotation(); + z = M * (y - e); // z = X.inv().act(y) - X.inv().act(e) + // = M * (y - e) + Z = E + M * R * M.transpose(); // Kalman gain K = P_L * H.transpose() * Z.inverse(); @@ -249,11 +252,11 @@ int main() dx = K * z; // Update - X = X.lplus(dx); + X = X.plus(-dx); P_L = (I - K * H) * P_L; // transform covariance from left to right invariant - P = AdXinv * P_L * AdXinv.transpose(); + P = AdX * P_L * AdX.transpose(); From 9eb9df3d59949d0fc54ede7b7af0e592e41c9fb2 Mon Sep 17 00:00:00 2001 From: chris7462 Date: Sat, 21 Sep 2024 19:54:35 -0400 Subject: [PATCH 5/6] Fix the update. --- examples/se2_localization_liekf.cpp | 18 +++++------------- 1 file changed, 5 insertions(+), 13 deletions(-) diff --git a/examples/se2_localization_liekf.cpp b/examples/se2_localization_liekf.cpp index db138be8..aa6740e2 100644 --- a/examples/se2_localization_liekf.cpp +++ b/examples/se2_localization_liekf.cpp @@ -133,7 +133,7 @@ int main() // Define the robot pose element and its covariance manif::SE2d X, X_simulation, X_unfiltered; - Matrix3d P, P_L; + Matrix3d P; X_simulation.setIdentity(); X.setIdentity(); @@ -151,7 +151,7 @@ int main() 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; + manif::SE2d::Jacobian J_x, J_u; // Define the gps measurements in R^2 Vector2d y, y_noise; @@ -228,16 +228,11 @@ int main() /// Then we correct using the gps position - - - - - - - - - - - - - - - - // transform covariance from right to left invariant - AdX = X.adj(); - AdXinv = X.inverse().adj(); - P_L = AdXinv * P * AdXinv.transpose(); - // expectation e = X.translation(); // e = t, for X = (R,t). H.topLeftCorner<2, 2>() = -Matrix2d::Identity(); H.topRightCorner<2, 1>() = Vector2d::Zero(); - E = H * P_L * H.transpose(); + E = H * P * H.transpose(); // innovation M = X.inverse().rotation(); @@ -246,17 +241,14 @@ int main() Z = E + M * R * M.transpose(); // Kalman gain - K = P_L * H.transpose() * Z.inverse(); + K = P * H.transpose() * Z.inverse(); // Correction step dx = K * z; // Update X = X.plus(-dx); - P_L = (I - K * H) * P_L; - - // transform covariance from left to right invariant - P = AdX * P_L * AdX.transpose(); + P = (I - K * H) * P; From 0ac0dcbdfb57ccbb63c04695ed0f3d98a8f49ba9 Mon Sep 17 00:00:00 2001 From: chris7462 Date: Tue, 24 Sep 2024 15:52:01 -0400 Subject: [PATCH 6/6] Finally fix based on the comment --- examples/se2_localization_liekf.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/se2_localization_liekf.cpp b/examples/se2_localization_liekf.cpp index aa6740e2..982dc31a 100644 --- a/examples/se2_localization_liekf.cpp +++ b/examples/se2_localization_liekf.cpp @@ -230,7 +230,7 @@ int main() // expectation e = X.translation(); // e = t, for X = (R,t). - H.topLeftCorner<2, 2>() = -Matrix2d::Identity(); + H.topLeftCorner<2, 2>() = Matrix2d::Identity(); H.topRightCorner<2, 1>() = Vector2d::Zero(); E = H * P * H.transpose(); @@ -247,7 +247,7 @@ int main() dx = K * z; // Update - X = X.plus(-dx); + X = X.plus(dx); P = (I - K * H) * P;