NavStateImuEKF.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/LeftLinearEKF.h>  // Include the base class
#include <gtsam/navigation/NavState.h>
#include <gtsam/navigation/PreintegrationParams.h>
 
namespace gtsam {

class GTSAM_EXPORT NavStateImuEKF : public LeftLinearEKF<NavState> {
 public:
  using Base = LeftLinearEKF<NavState>;
  using TangentVector = typename Base::TangentVector;  // Vector9
  using Jacobian = typename Base::Jacobian;            // 9x9
  using Covariance = typename Base::Covariance;        // 9x9

  NavStateImuEKF(const NavState& X0, const Covariance& P0,
                 const std::shared_ptr<PreintegrationParams>& params);

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

  static NavState Imu(const Vector3& omega_b, const Vector3& f_b, double dt);

  static NavState Dynamics(const Vector3& g_n, const NavState& X,
                           const Vector3& omega_b, const Vector3& f_b,
                           double dt, OptionalJacobian<9, 9> 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_;
  Covariance Q_ = Covariance::Zero();
};
 
}  // namespace gtsam