Merge pull request #2315 from borglab/fix/filters

Various filter changes

Author: dellaert

examples/GEKF_Rot3Example.cpp

@@ -57,15 +57,16 @@ int main() {
 
   // Timestep, process noise, measurement noise
   double dt = 0.1;
-  Matrix3 Q = Matrix3::Identity() * 0.01;
+  // Continuous-time process noise (scaled by dt inside predict).
+  Matrix3 Qc = Matrix3::Identity() * 0.1;
   Matrix3 Rm = Matrix3::Identity() * 0.05;
 
   cout << "=== Init ===\nR:\n"
        << ekf.state().matrix() << "\nP:\n"
        << ekf.covariance() << "\n\n";
 
   // Predict using state‐dependent f
-  ekf.predict(dynamicsSO3, dt, Q);
+  ekf.predict(dynamicsSO3, dt, Qc);
   cout << "--- After predict ---\nR:\n" << ekf.state().matrix() << "\n\n";
 
   // Magnetometer measurement = body‐frame reading of m_world

examples/IEKF_NavstateExample.cpp

@@ -58,8 +58,9 @@ int main() {
 
   // Noise & timestep
   double dt = 1.0;
-  Matrix9 Q = Matrix9::Identity() * 0.01;
-  Matrix3 R = Matrix3::Identity() * 0.5;
+  // Continuous-time process noise (scaled by dt inside predict)
+  Matrix9 Qc = Matrix9::Identity() * 0.01;
+  Matrix3 R = Matrix3::Identity() * 0.5;
 
   // Two IMU samples [a; ω]
   Vector6 imu1;
@@ -77,15 +78,15 @@ int main() {
     << "\n\n";
 
   // --- first predict/update ---
-  ekf.predict(dynamics(imu1), dt, Q);
+  ekf.predict(dynamics(imu1), dt, Qc);
   cout << "--- After predict 1 ---\nX: " << ekf.state()
     << "\nP: " << ekf.covariance() << "\n\n";
   ekf.update(h_gps, z1, R);
   cout << "--- After update 1 ---\nX: " << ekf.state()
     << "\nP: " << ekf.covariance() << "\n\n";
 
   // --- second predict/update ---
-  ekf.predict(dynamics(imu2), dt, Q);
+  ekf.predict(dynamics(imu2), dt, Qc);
   cout << "--- After predict 2 ---\nX: " << ekf.state()
     << "\nP: " << ekf.covariance() << "\n\n";
   ekf.update(h_gps, z2, R);

gtsam/geometry/Rot2.cpp

@@ -81,6 +81,23 @@ Vector1 Rot2::Logmap(const Rot2& r, OptionalJacobian<1, 1> H) {
   v << r.theta();
   return v;
 }
+
+/* ************************************************************************* */
+Matrix1 Rot2::adjointMap(const Vector1&) {
+  Matrix1 ad;
+  ad << 0.0;
+  return ad;
+}
+
+/* ************************************************************************* */
+Vector1 Rot2::adjoint(const Vector1&, const Vector1&,
+                      OptionalJacobian<1, 1> Hxi,
+                      OptionalJacobian<1, 1> Hy) {
+  if (Hxi) *Hxi = Matrix1::Zero();
+  if (Hy) *Hy = Matrix1::Zero();
+  return Vector1::Zero();
+}
+
 /* ************************************************************************* */
 Matrix2 Rot2::Hat(const Vector1& xi) {
   Matrix2 X;

gtsam/geometry/Rot2.h

@@ -135,6 +135,14 @@ namespace gtsam {
     /** Calculate Adjoint map */
     Matrix1 AdjointMap() const { return I_1x1; }
 
+    /// Lie-algebra adjoint (zero for abelian SO(2)).
+    static Matrix1 adjointMap(const Vector1&);
+
+    /// Apply Lie-algebra adjoint (always zero).
+    static Vector1 adjoint(const Vector1&, const Vector1&,
+                           OptionalJacobian<1, 1> Hxi = {},
+                           OptionalJacobian<1, 1> Hy = {});
+
     /// Left-trivialized derivative of the exponential map
     static Matrix ExpmapDerivative(const Vector& /*v*/) {
       return I_1x1;

gtsam/geometry/Rot3.cpp

@@ -245,6 +245,19 @@ Matrix3 Rot3::LogmapDerivative(const Vector3& x)    {
   return SO3::LogmapDerivative(x);
 }
 
+/* ************************************************************************* */
+Matrix3 Rot3::adjointMap(const Vector3& xi) { return Hat(xi); }
+
+/* ************************************************************************* */
+Vector3 Rot3::adjoint(const Vector3& xi, const Vector3& y,
+                      OptionalJacobian<3, 3> Hxi,
+                      OptionalJacobian<3, 3> Hy) {
+  const Matrix3 ad_xi = adjointMap(xi);
+  if (Hxi) *Hxi = -Hat(y);
+  if (Hy) *Hy = ad_xi;
+  return ad_xi * y;
+}
+
 /* ************************************************************************* */
 pair<Matrix3, Vector3> RQ(const Matrix3& A, OptionalJacobian<3, 9> H) {
   const double x = -atan2(-A(2, 1), A(2, 2));
@@ -319,4 +332,3 @@ Rot3 Rot3::slerp(double t, const Rot3& other) const {
 /* ************************************************************************* */
 
 } // namespace gtsam
-

gtsam/geometry/Rot3.h

@@ -396,6 +396,14 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
     /** Calculate Adjoint map */
     Matrix3 AdjointMap() const { return matrix(); }
 
+    /// Matrix representation of the Lie-algebra adjoint operator ad_xi on so(3).
+    static Matrix3 adjointMap(const Vector3& xi);
+
+    /// Apply the Lie-algebra adjoint map to y with optional derivatives.
+    static Vector3 adjoint(const Vector3& xi, const Vector3& y,
+                           OptionalJacobian<3, 3> Hxi = {},
+                           OptionalJacobian<3, 3> Hy = {});
+
     // Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE
     struct GTSAM_EXPORT ChartAtOrigin {
       static Rot3 Retract(const Vector3& v, ChartJacobian H = {});
@@ -599,4 +607,3 @@ template <>
 struct traits<const Rot3> : public internal::MatrixLieGroup<Rot3, 3> {};
 
 }  // namespace gtsam
-

gtsam/navigation/InvariantEKF.h

@@ -43,6 +43,11 @@ namespace gtsam {
  * 2. Prediction via tangent control vector: `predict(const TangentVector& u,
  * double dt, const Covariance& Q)`
  *
+ *
+ * Noise convention mirrors LieGroupEKF:
+ * - `predict(U, Q)` expects `Q` to be a discrete covariance.
+ * - `predict(u, dt, Q)` interprets `Q` as continuous-time and scales by `dt`.
+ *
  * The state-dependent prediction methods from LeftLinearEKF are hidden.
  * The update step remains the same as in ManifoldEKF/LeftLinearEKF.
  * For details on how static and dynamic dimensions are handled, please refer to
@@ -107,7 +112,7 @@ class InvariantEKF : public LeftLinearEKF<G> {
    *
    * @param u Tangent space control vector.
    * @param dt Time interval.
-   * @param Q Process noise covariance matrix.
+   * @param Q Continuous-time process noise covariance matrix (scaled by dt).
    */
   void predict(const TangentVector& u, double dt, const Covariance& Q) {
     G U;
@@ -119,7 +124,7 @@ class InvariantEKF : public LeftLinearEKF<G> {
     } else {
       U = traits<G>::Expmap(u * dt);
     }
-    predict(U, Q);  // Call the group composition predict
+    predict(U, Q * dt);  // Q interpreted as continuous-time; discretize with dt
   }
 
   /**