LeftLinearEKF.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/LieGroupEKF.h>  // Include the base class
 
#include <type_traits>  // For std::conjunction, std::is_invocable_r, std::is_same
 
namespace gtsam {

template <typename G>
class LeftLinearEKF : public LieGroupEKF<G> {
 public:
  using Base = LieGroupEKF<G>;
  static constexpr int Dim = Base::Dim;  
  using TangentVector = typename Base::TangentVector;
  using Jacobian = typename Base::Jacobian;
  using Covariance = typename Base::Covariance;
 
  LeftLinearEKF(const G& X0, const Covariance& P0) : Base(X0, P0) {}

  template <typename Phi>
  struct is_automorphism
      : std::conjunction<
            std::is_invocable_r<G, Phi, const G&>,
            std::is_same<decltype(std::declval<Phi>().dIdentity()), Jacobian>> {
  };

  template <class Phi, typename = std::enable_if_t<is_automorphism<Phi>::value>>
  static G Dynamics(const G& W, const Phi& phi, const G& X, const G& U,
                    OptionalJacobian<Dim, Dim> A = {}) {
    return traits<G>::Compose(W, Dynamics<Phi>(phi, X, U, A));
  }

  template <class Phi, typename = std::enable_if_t<is_automorphism<Phi>::value>>
  static G Dynamics(const Phi& phi, 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) * phi.dIdentity();
    }
    return traits<G>::Compose(phi(X), U);
  }

  template <class Phi, typename = std::enable_if_t<is_automorphism<Phi>::value>>
  void predict(const G& W, const Phi& phi, const G& U, const Covariance& Q) {
    Jacobian A;
    this->X_ = this->Dynamics(W, phi, this->X_, U, A);
    this->P_ = A * this->P_ * A.transpose() + Q;
  }

  template <class Phi, typename = std::enable_if_t<is_automorphism<Phi>::value>>
  void predict(const Phi& phi, const G& U, const Covariance& Q) {
    Jacobian A;
    this->X_ = this->Dynamics(phi, this->X_, U, A);
    this->P_ = A * this->P_ * A.transpose() + Q;
  }
};
 
}  // namespace gtsam