From 754b1fcb53bacd6e63cb006c89564fb3ce80d223 Mon Sep 17 00:00:00 2001
From: chris7462 <chris7462@gmail.com>
Date: Wed, 11 Sep 2024 07:32:17 -0400
Subject: [PATCH] Fix Jacobian based on the discussion

---
 examples/se2_localization_liekf.cpp | 24 +++++++++++++-----------
 1 file changed, 13 insertions(+), 11 deletions(-)

diff --git a/examples/se2_localization_liekf.cpp b/examples/se2_localization_liekf.cpp
index 522c6c2d..6bd33ffb 100644
--- a/examples/se2_localization_liekf.cpp
+++ b/examples/se2_localization_liekf.cpp
@@ -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
@@ -157,8 +157,9 @@ int main()
     Vector2d    y, y_noise;
     Array2d     y_sigmas;
     Matrix2d    R;
+    Matrix2d    M;
 
-    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
@@ -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,18 @@ 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 = E + M * R * M.transpose();
 
         // Kalman gain
         K = P_L * H.transpose() * Z.inverse();
@@ -249,11 +251,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();