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/Manifold.h>  // Include for traits and IsManifold
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
 
#include <stdexcept>
#include <string>
#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.");
 
    n_ = traits<M>::GetDimension(X0);
    if constexpr (Dim == Eigen::Dynamic) {
      // Validate dimensions of initial covariance P0.
      if (!isMatrixOfSize(P0, n_, 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_) + ").");
      }
      I_ = Jacobian::Identity(n_, n_);
    } else {
      I_ = Jacobian::Identity();
    }
 
    P_ = P0;
  }
 
  virtual ~ManifoldEKF() = default;

  const M& state() const { return X_; }

  const Covariance& covariance() const { return P_; }

  size_t dimension() const { return n_; }

  void predict(const M& X_next, const Jacobian& F, const Covariance& Q) {
    if constexpr (Dim == Eigen::Dynamic) {
      if (!isMatrixOfSize(F, n_, n_) || !isMatrixOfSize(Q, n_, 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 HMatrix, typename RMatrix>
  auto KalmanGain(const HMatrix& H, const RMatrix& R) const {
    auto S = H * P_ * H.transpose() + R;  // Innovation covariance
    return P_ * H.transpose() * S.inverse();
  }

  template <typename GainMatrix, typename HMatrix, typename RMatrix>
  void JosephUpdate(const GainMatrix& K, const HMatrix& H, const RMatrix& R) {
    Jacobian I_KH = I_ - K * H;
    P_ = I_KH * P_ * I_KH.transpose() + K * R * K.transpose();
  }

  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,
      bool performReset = true) {
    static constexpr int MeasDim = traits<Measurement>::dimension;
 
    // Innovation: y = h(x_pred) - z. In tangent space: local(z, h(x_pred))
    // NOTE: we use the `z_hat - z` sign convention, NOT `z - z_hat`.
    typename traits<Measurement>::TangentVector innovation =
        traits<Measurement>::Local(z, prediction);
 
    // Kalman Gain: K = P H^T S^-1
    // K will be Eigen::Matrix<double, Dim, MeasDim>
    Eigen::Matrix<double, Dim, MeasDim> K = KalmanGain(H, R);
 
    // 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 covariance in the tangent space at the current state
    this->JosephUpdate(K, H, R);
 
    // Update state using retract/ transport or just retract
    if (performReset)
      reset(delta_xi);
    else
      X_ = traits<M>::Retract(X_, delta_xi);
  }

  template <typename Measurement, typename MeasurementFunction>
  void update(MeasurementFunction&& h, const Measurement& z,
              const Eigen::Matrix<double, traits<Measurement>::dimension,
                                  traits<Measurement>::dimension>& R,
              bool performReset = true) {
    static_assert(IsManifold<Measurement>::value,
                  "Template parameter Measurement must be a GTSAM Manifold.");
 
    // Predict measurement and get Jacobian H = dh/dlocal(X)
    Matrix H(traits<Measurement>::GetDimension(z), n_);
    Measurement prediction = h(X_, H);
 
    // Call the other update function
    update<Measurement>(prediction, H, z, R, performReset);
  }

  void updateWithVector(const gtsam::Vector& prediction, const Matrix& H,
                        const gtsam::Vector& z, const Matrix& R,
                        bool performReset = true) {
    validateInputs(prediction, H, z, R);
    update<Vector>(prediction, H, z, R, performReset);
  }

  void reset(const TangentVector& eta) {
    if constexpr (HasRetractJacobian<M>::value) {
      Jacobian B;
      if constexpr (Dim == Eigen::Dynamic) B.resize(n_, n_);
      X_ = traits<M>::Retract(X_, eta, &B);
      P_ = B * P_ * B.transpose();
    } else {
      X_ = traits<M>::Retract(X_, eta);
      // Covariance unchanged when Jacobian is not available.
    }
  }
 
 protected:
  void validateInputs(const gtsam::Vector& prediction, const Matrix& H,
                      const gtsam::Vector& z, const Matrix& R) {
    const size_t m = static_cast<size_t>(prediction.size());
    if (static_cast<size_t>(z.size()) != m) {
      throw std::invalid_argument(
          "ManifoldEKF::updateWithVector: prediction and z must have same "
          "length.");
    }
    if (!isMatrixOfSize(H, m, n_)) {
      throw std::invalid_argument(
          "ManifoldEKF::updateWithVector: H must be m x n where m = "
          "measurement size and n = state dimension.");
    }
    if (!isMatrixOfSize(R, m, m)) {
      throw std::invalid_argument(
          "ManifoldEKF::updateWithVector: R must be m x m where m = "
          "measurement size.");
    }
  }

  template <typename MatrixType>
  static bool isMatrixOfSize(const MatrixType& matrix, size_t rows,
                             size_t cols) {
    return static_cast<size_t>(matrix.rows()) == rows &&
           static_cast<size_t>(matrix.cols()) == cols;
  }
 
 protected:
  M X_;           
  Covariance P_;  
  Jacobian I_;    
  size_t n_;      
 
 private:
  // Detection helper: check if traits<T>::Retract(x, v, Jacobian*) is valid.
  template <typename T, typename = void>
  struct HasRetractJacobian : std::false_type {};
  template <typename T>
  struct HasRetractJacobian<
      T, std::void_t<decltype(traits<T>::Retract(
             std::declval<const T&>(),
             std::declval<const typename traits<T>::TangentVector&>(),
             (Jacobian*)nullptr))>> : std::true_type {};
};
 
}  // namespace gtsam