ManifoldEKF.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * 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
 
 * -------------------------------------------------------------------------- */
 
#pragma once
 
#include <gtsam/base/Matrix.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Manifold.h> // Include for traits and IsManifold
 
#include <Eigen/Dense>
#include <string>
#include <stdexcept>
#include <type_traits>
 
namespace gtsam {
 
  template <typename M>
  class ManifoldEKF {
  public:
    static constexpr int Dim = traits<M>::dimension;
 
    using TangentVector = typename traits<M>::TangentVector;
    using Covariance = Eigen::Matrix<double, Dim, Dim>;
    using Jacobian = Eigen::Matrix<double, Dim, Dim>;
 
 
    ManifoldEKF(const M& X0, const Covariance& P0) : X_(X0) {
      static_assert(IsManifold<M>::value,
        "Template parameter M must be a GTSAM Manifold.");
 
      if constexpr (Dim == Eigen::Dynamic) {
        n_ = traits<M>::GetDimension(X0);
        // Validate dimensions of initial covariance P0.
        if (P0.rows() != n_ || P0.cols() != n_) {
          throw std::invalid_argument(
            "ManifoldEKF: Initial covariance P0 dimensions (" +
            std::to_string(P0.rows()) + "x" + std::to_string(P0.cols()) +
            ") do not match state's tangent space dimension (" +
            std::to_string(n_) + ").");
        }
      }
      else {
        n_ = Dim;
      }
 
      P_ = P0;
    }
 
    virtual ~ManifoldEKF() = default;
 
    const M& state() const { return X_; }
 
    const Covariance& covariance() const { return P_; }
 
    int dimension() const { return n_; }
 
    void predict(const M& X_next, const Jacobian& F, const Covariance& Q) {
      if constexpr (Dim == Eigen::Dynamic) {
        if (F.rows() != n_ || F.cols() != n_ || Q.rows() != n_ || Q.cols() != n_) {
          throw std::invalid_argument(
            "ManifoldEKF::predict: Dynamic F/Q dimensions must match state dimension " +
            std::to_string(n_) + ".");
        }
      }
 
      X_ = X_next;
      P_ = F * P_ * F.transpose() + Q;
    }
 
    template <typename Measurement>
    void update(const Measurement& prediction,
      const Eigen::Matrix<double, traits<Measurement>::dimension, Dim>& H,
      const Measurement& z,
      const Eigen::Matrix<double, traits<Measurement>::dimension, traits<Measurement>::dimension>& R) {
 
      static_assert(IsManifold<Measurement>::value,
        "Template parameter Measurement must be a GTSAM Manifold for LocalCoordinates.");
 
      static constexpr int MeasDim = traits<Measurement>::dimension;
 
      int m_runtime;
      if constexpr (MeasDim == Eigen::Dynamic) {
        m_runtime = traits<Measurement>::GetDimension(z);
        if (traits<Measurement>::GetDimension(prediction) != m_runtime) {
          throw std::invalid_argument(
            "ManifoldEKF::update: Dynamic measurement 'prediction' and 'z' have different dimensions.");
        }
        if (H.rows() != m_runtime || H.cols() != n_ || R.rows() != m_runtime || R.cols() != m_runtime) {
          throw std::invalid_argument(
            "ManifoldEKF::update: Jacobian H or Noise R dimensions mismatch for dynamic measurement.");
        }
      }
      else {
        m_runtime = MeasDim;
        if constexpr (Dim == Eigen::Dynamic) {
          if (H.cols() != n_) {
            throw std::invalid_argument(
              "ManifoldEKF::update: Jacobian H columns must match state dimension " + std::to_string(n_) + ".");
          }
        }
      }
 
      // Innovation: y = z - h(x_pred). In tangent space: local(h(x_pred), z)
      typename traits<Measurement>::TangentVector innovation =
        traits<Measurement>::Local(prediction, z);
 
      // Innovation covariance: S = H P H^T + R
      // S will be Eigen::Matrix<double, MeasDim, MeasDim>
      Eigen::Matrix<double, MeasDim, MeasDim> S = H * P_ * H.transpose() + R;
 
      // Kalman Gain: K = P H^T S^-1
      // K will be Eigen::Matrix<double, Dim, MeasDim>
      Eigen::Matrix<double, Dim, MeasDim> K = P_ * H.transpose() * S.inverse();
 
      // Correction vector in tangent space of M: delta_xi = K * innovation
      const TangentVector delta_xi = K * innovation; // delta_xi is Dim x 1 (or n_ x 1 if dynamic)
 
      // Update state using retract: X_new = retract(X_old, delta_xi)
      X_ = traits<M>::Retract(X_, delta_xi);
 
      // Update covariance using Joseph form for numerical stability
      Jacobian I_n; // Eigen::Matrix<double, Dim, Dim>
      if constexpr (Dim == Eigen::Dynamic) {
        I_n = Jacobian::Identity(n_, n_);
      }
      else {
        I_n = Jacobian::Identity();
      }
 
      // I_KH will be Eigen::Matrix<double, Dim, Dim>
      Jacobian I_KH = I_n - K * H;
      P_ = I_KH * P_ * I_KH.transpose() + K * R * K.transpose();
    }
 
    template <typename Measurement, typename MeasurementFunction>
    void update(MeasurementFunction&& h, const Measurement& z,
      const Eigen::Matrix<double, traits<Measurement>::dimension, traits<Measurement>::dimension>& R) {
      static_assert(IsManifold<Measurement>::value,
        "Template parameter Measurement must be a GTSAM Manifold.");
 
      static constexpr int MeasDim = traits<Measurement>::dimension;
 
      int m_runtime;
      if constexpr (MeasDim == Eigen::Dynamic) {
        m_runtime = traits<Measurement>::GetDimension(z);
      }
      else {
        m_runtime = MeasDim;
      }
 
      // Predict measurement and get Jacobian H = dh/dlocal(X)
      Matrix H(m_runtime, n_);
      Measurement prediction = h(X_, H);
 
      // Call the other update function
      update(prediction, H, z, R);
    }
 
  protected:
    M X_;              
    Covariance P_;     
    int n_;            
  };
 
}  // namespace gtsam