InvariantEKF.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>  // For traits (needed for AdjointMap, Expmap)
#include <gtsam/navigation/LeftLinearEKF.h>  // Include the base class
 
namespace gtsam {

template <typename G>
class InvariantEKF : public LeftLinearEKF<G> {
 public:
  using Base = LeftLinearEKF<G>;         
  static constexpr int Dim = Base::Dim;  
  using TangentVector = typename Base::TangentVector;  
  using Jacobian = typename Base::Jacobian;
  using Covariance = typename Base::Covariance;

  InvariantEKF(const G& X0, const Covariance& P0) : Base(X0, P0) {}

  static G Dynamics(const G& X, const G& U, OptionalJacobian<Dim, Dim> A = {}) {
    if (A) {
      const G U_inv = traits<G>::Inverse(U);
      *A = traits<G>::AdjointMap(U_inv);
    }
    return traits<G>::Compose(X, U);
  }
 
  // We hide state-dependent predict methods from LeftLinearEKF by only
  // providing the invariant predict methods below.

  void predict(const G& U, const Covariance& Q) {
    this->X_ = traits<G>::Compose(this->X_, U);
    const G U_inv = traits<G>::Inverse(U);
    const Jacobian A = traits<G>::AdjointMap(U_inv);
    // P_ is Covariance. A is Jacobian. Q is Covariance.
    // All are Eigen::Matrix<double,Dim,Dim>.
    this->P_ = A * this->P_ * A.transpose() + Q;
  }

  void predict(const TangentVector& u, double dt, const Covariance& Q) {
    G U;
    if constexpr (std::is_same_v<G, Matrix>) {
      // Specialize to Matrix case as its Expmap is not defined.
      const Matrix& X = static_cast<const Matrix&>(this->X_);
      U.resize(X.rows(), X.cols());
      Eigen::Map<Vector>(static_cast<Matrix&>(U).data(), U.size()) = u * dt;
    } else {
      U = traits<G>::Expmap(u * dt);
    }
    predict(U, Q * dt);  // Q interpreted as continuous-time; discretize with dt
  }

  static G Dynamics(const G& W, const G& X, const G& U,
                    OptionalJacobian<Dim, Dim> A = {}) {
    return traits<G>::Compose(W, Dynamics(X, U, A));  // A is independent of W
  }

  void predict(const G& W, const G& U, const Covariance& Q) {
    predict(U, Q);  // First apply U
    // Then apply W on the left
    this->X_ = traits<G>::Compose(W, this->X_);
  }
 
};  // InvariantEKF
 
}  // namespace gtsam