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/navigation/LieGroupEKF.h> // Include the base class
#include <gtsam/base/Lie.h> // For traits (needed for AdjointMap, Expmap)
 
 
namespace gtsam {
 
  template <typename G>
  class InvariantEKF : public LieGroupEKF<G> {
  public:
    using Base = LieGroupEKF<G>; 
    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) {}
 
    // We hide state-dependent predict methods from LieGroupEKF 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); // Call the group composition predict
    }
 
  }; // InvariantEKF
 
} // namespace gtsam