LieGroupEKF.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/Lie.h>  // Include for Lie group traits and operations
#include <gtsam/base/VectorSpace.h>
#include <gtsam/navigation/ManifoldEKF.h>  // Include the base class
 
#include <functional>  // For std::function
#include <type_traits>
#include <utility>
 
namespace gtsam {

template <typename G>
class LieGroupEKF : public ManifoldEKF<G> {
 public:
  using This = LieGroupEKF<G>;
  using Base = ManifoldEKF<G>;           
  static constexpr int Dim = Base::Dim;  
 
  using Jacobian = typename Base::Jacobian;            
  using Covariance = typename Base::Covariance;        
  using TangentVector = typename Base::TangentVector;  
 
 private:
  template <typename Dynamics>
  using enable_if_dynamics =
      std::enable_if_t<!std::is_convertible_v<Dynamics, TangentVector> &&
                       std::is_invocable_r_v<TangentVector, Dynamics, const G&,
                                             OptionalJacobian<Dim, Dim>&>>;

  template <typename Control, typename Dynamics>
  using enable_if_full_dynamics = std::enable_if_t<
      std::is_invocable_r_v<TangentVector, Dynamics, const G&, const Control&,
                            OptionalJacobian<Dim, Dim>&>>;
 
  template <typename T, typename = void>
  struct has_adjoint_map : std::false_type {};
 
  template <typename T>
  struct has_adjoint_map<
      T, std::void_t<decltype(T::adjointMap(
             std::declval<typename traits<T>::TangentVector>()))>>
      : std::true_type {};
 
 public:
  LieGroupEKF(const G& X0, const Covariance& P0) : Base(X0, P0) {
    static_assert(IsLieGroup<G>::value,
                  "Template parameter G must be a GTSAM Lie Group");
  }

  using Base::predict;

  template <size_t K = 1>
  Jacobian transitionMatrix(const TangentVector& xi, const Jacobian& Df,
                            double dt, const G& U, const Jacobian& Dexp) const {
    if constexpr (std::is_same_v<G, Matrix>) {
      (void)xi;
      (void)U;
      (void)Dexp;
      return expm(Df * dt, K);
    } else {
      if constexpr (K == 1) {
        // First-order Lie group transition matrix
        return traits<G>::Inverse(U).AdjointMap() + Dexp * Df * dt;
      } else {
        // Higher-order Lie group transition matrix via matrix exponential.
        //
        // GTSAM's LieGroupEKF uses a left-invariant error, i.e. we write the
        // true state as
        //
        //     X_true ≈ X_hat * Exp(η),
        //
        // and η lives in the tangent space at the identity. Even if the
        // *global* perturbation is constant, composing the nominal motion X_hat
        // with Exp(ξ dt) on the right will "drag" η along the group because G
        // is non-commutative. The Baker–Campbell–Hausdorff expansion shows that
        // the linearized error dynamics contain an extra term -ad_ξ η coming
        // from this non-commutativity, where ad_ξ(·) = [ξ, ·] is the Lie
        // bracket. With this convention, and a dynamics linearization Df =
        // ∂ξ/∂(local X), the continuous-time error satisfies approximately
        //
        //     dη/dt ≈ (Df - ad_ξ) η,
        //
        // and the corresponding discrete-time transition matrix is
        //
        //     Φ ≈ expm((Df - ad_ξ) dt).
        //
        // This is exactly what we implement below.
        static_assert(
            has_adjoint_map<G>::value,
            "transitionMatrix<K> requires G::adjointMap(xi) when K > 1.");
        Jacobian ad_xi = G::adjointMap(xi);
        const Matrix A = Df - ad_xi;
        return expm(A * dt, K);
      }
    }
  }

  template <size_t K = 1, typename Dynamics,
            typename = enable_if_dynamics<Dynamics>>
  G predictMean(Dynamics&& f, double dt,
                OptionalJacobian<Dim, Dim> Phi = {}) const {
    if (Phi) {
      Jacobian Df;
      TangentVector xi = f(this->X_, &Df);
      if constexpr (std::is_same_v<G, Matrix>) {
        *Phi = expm(Df * dt, K);
        return traits<G>::Retract(this->X_, xi * dt);
      } else {
        Jacobian Dexp;
        G U = traits<G>::Expmap(xi * dt, &Dexp);
        *Phi = transitionMatrix<K>(xi, Df, dt, U, Dexp);
        return traits<G>::Compose(this->X_, U);
      }
    } else {
      TangentVector xi = f(this->X_, nullptr);
      if constexpr (std::is_same_v<G, Matrix>) {
        return traits<G>::Retract(this->X_, xi * dt);
      } else {
        G U = traits<G>::Expmap(xi * dt);
        return traits<G>::Compose(this->X_, U);
      }
    }
  }

  template <size_t K = 1, typename Dynamics,
            typename = enable_if_dynamics<Dynamics>>
  void predict(Dynamics&& f, double dt, const Covariance& Q) {
    Jacobian Phi;
    if constexpr (Dim == Eigen::Dynamic) {
      Phi.resize(this->n_, this->n_);
    }
    G X_next = predictMean<K>(std::forward<Dynamics>(f), dt, Phi);
    predict(X_next, Phi,
            Q * dt);  // Q interpreted as continuous-time; discretize with dt
  }

  template <size_t K = 1, typename Control, typename Dynamics,
            typename = enable_if_full_dynamics<Control, Dynamics>>
  G predictMean(Dynamics&& f, const Control& u, double dt,
                OptionalJacobian<Dim, Dim> Phi = {}) const {
    return predictMean<K>(
        [&](const G& X, OptionalJacobian<Dim, Dim> Df) { return f(X, u, Df); },
        dt, Phi);
  }

  template <size_t K = 1, typename Control, typename Dynamics,
            typename = enable_if_full_dynamics<Control, Dynamics>>
  void predict(Dynamics&& f, const Control& u, double dt, const Covariance& Q) {
    return predict<K>(
        [&](const G& X, OptionalJacobian<Dim, Dim> Df) { return f(X, u, Df); },
        dt, Q);
  }

  void predictWithCompose(const G& U, const Jacobian& J_UX,
                          const Covariance& Q) {
    Jacobian A_local;
    if constexpr (std::is_same_v<G, Matrix>) {
      const Matrix& I_n = this->I_;
      A_local = I_n + J_UX;
      this->X_ = traits<Matrix>::Retract(this->X_, U);
    } else {
      A_local = traits<G>::Inverse(U).AdjointMap() + J_UX;
      this->X_ = this->X_.compose(U);
    }
    this->P_ = A_local * this->P_ * A_local.transpose() + Q;
  }

  using Base::update;
 
};  // LieGroupEKF
 
}  // namespace gtsam