Gal3ImuEKF.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/VectorConstants.h>
#include <gtsam/geometry/Gal3.h>
#include <gtsam/navigation/InvariantEKF.h>  // Include the base class
#include <gtsam/navigation/PreintegrationParams.h>
 
namespace gtsam {

class GTSAM_EXPORT Gal3ImuEKF : public InvariantEKF<Gal3> {
 public:
  using Base = InvariantEKF<Gal3>;
  using TangentVector = typename Base::TangentVector;  // Vector10
  using Jacobian = typename Base::Jacobian;            // 10x10
  using Covariance = typename Base::Covariance;        // 10x10

  enum Mode {
    NO_TIME,  
    TRACK_TIME_NO_COVARIANCE,  
    TRACK_TIME_WITH_COVARIANCE,  
  };

  Gal3ImuEKF(const Gal3& X0, const Covariance& P0,
             const std::shared_ptr<PreintegrationParams>& params,
             Mode mode = TRACK_TIME_NO_COVARIANCE);

  static Gal3 Gravity(const Vector3& g_n, double dt) {
    return {Rot3(), g_n * (0.5 * dt * dt), g_n * dt, 0.0};
  }

  static Gal3 TimeZeroingGravity(const Vector3& g_n, double dt) {
    return {Rot3(), -g_n * (0.5 * dt * dt), g_n * dt, -dt};
  }

  static Gal3 CompensatedGravity(const Vector3& g_n, double dt, double t_k) {
    const Point3 pW(-t_k * g_n * dt - g_n * (0.5 * dt * dt));
    const Vector3 vW = g_n * dt;
    return {Rot3(), pW, vW, 0.0};
  }

  static Gal3 Imu(const Vector3& omega_b, const Vector3& f_b, double dt) {
    Gal3::TangentVector xi;
    xi << omega_b, f_b, Z_3x1, 1.0;
    return Gal3::Expmap(xi * dt);
  }

  static Gal3 Dynamics(const Vector3& g_n, const Gal3& X,
                       const Vector3& omega_b, const Vector3& f_b, double dt,
                       Mode mode = TRACK_TIME_WITH_COVARIANCE,
                       OptionalJacobian<10, 10> A = {});

  void predict(const Vector3& omega_b, const Vector3& f_b, double dt);

  const std::shared_ptr<PreintegrationParams>& params() const;
  const Vector3& gravity() const;
  const Covariance& processNoise() const;
 
 private:
  std::shared_ptr<PreintegrationParams> params_;
  Mode mode_{TRACK_TIME_NO_COVARIANCE};
  Covariance Q_ = Covariance::Zero();
};
 
}  // namespace gtsam