Detailed Description

Specialized EKF for IMU-driven NavState on SE_2(3).

Inheritance diagram for gtsam::NavStateImuEKF:

Public Member Functions
	NavStateImuEKF (const NavState &X0, const Covariance &P0, const std::shared_ptr< PreintegrationParams > &params)
	Construct with initial state/covariance and preintegration params (for gravity and IMU covariances).
void	predict (const Vector3 &omega_b, const Vector3 &f_b, double dt)
	Predict the next state using gyro and accelerometer measurements.
const std::shared_ptr< PreintegrationParams > &	params () const
	Accessors.
const Vector3 &	gravity () const
const Covariance &	processNoise () const
Public Member Functions inherited from gtsam::LeftLinearEKF< NavState >
	LeftLinearEKF (const NavState &X0, const Covariance &P0)
void	predict (const NavState &W, const Phi &phi, const NavState &U, const Covariance &Q)
	General left–linear prediction, updates filter state as follows: X⁺ = W · φ(X) · U P⁺ = A P Aᵀ + Q with A = Ad_{U^{-1}} Φ, Φ := dφ\|_e.
void	predict (const Phi &phi, const NavState &U, const Covariance &Q)
	Special case of predict with W=I, updates filter state as follows: Update: X⁺ = φ(X) · U Covariance: P⁺ = A P Aᵀ + Q with A = Ad_{U^{-1}} Φ, Φ := dφ\|_e.
Public Member Functions inherited from gtsam::LieGroupEKF< G >
	LieGroupEKF (const G &X0, const Covariance &P0)
	Constructor: initialize with state and covariance.
template<size_t K = 1>
Jacobian	transitionMatrix (const TangentVector &xi, const Jacobian &Df, double dt, const G &U, const Jacobian &Dexp) const
	Compute the discrete-time transition matrix Φ corresponding to a continuous-time linearization (Df) over time dt.
template<size_t K = 1, typename Dynamics, typename = enable_if_dynamics<Dynamics>>
G	predictMean (Dynamics &&f, double dt, OptionalJacobian< Dim, Dim > Phi={}) const
	Predict mean and Jacobian Phi with state-dependent dynamics: xi = f(X_k, Df) (tangent vector dynamics and Jacobian Df) U = Expmap(xi * dt, Dexp) (motion increment U and Expmap Jacobian Dexp) X_{k+1} = X_k * U (Predict next state via compose) Phi = Ad_{U^{-1}} + Dexp * Df * dt (K=1 first-order Jacobian) expm((Df - ad(xi))dt) (K>1 matrix exponential discretization).
template<size_t K = 1, typename Dynamics, typename = enable_if_dynamics<Dynamics>>
void	predict (Dynamics &&f, double dt, const Covariance &Q)
	Predict step with state-dependent dynamics: Uses predictMean to compute X_{k+1} and Phi, then updates covariance.
template<size_t K = 1, typename Control, typename Dynamics, typename = enable_if_full_dynamics<Control, Dynamics>>
G	predictMean (Dynamics &&f, const Control &u, double dt, OptionalJacobian< Dim, Dim > Phi={}) const
	Predict mean and Jacobian A with state and control input dynamics: Wraps the dynamics function and calls the state-only predictMean.
template<size_t K = 1, typename Control, typename Dynamics, typename = enable_if_full_dynamics<Control, Dynamics>>
void	predict (Dynamics &&f, const Control &u, double dt, const Covariance &Q)
	Predict step with state and control input dynamics: Wraps the dynamics function and calls the state-only predict.
void	predictWithCompose (const G &U, const Jacobian &J_UX, const Covariance &Q)
	Predict using a precomputed group increment U and its Jacobian J_UX.
void	predict (const G &X_next, const Jacobian &F, const Covariance &Q)
	Expose base class predict method, predict(const M& X_next, const Jacobian& F, const Covariance& Q).
void	update (const Measurement &prediction, const Eigen::Matrix< double, traits< Measurement >::dimension, Dim > &H, const Measurement &z, const Eigen::Matrix< double, traits< Measurement >::dimension, traits< Measurement >::dimension > &R, bool performReset=true)
	Update overloads follow ManifoldEKF.
void	update (MeasurementFunction &&h, const Measurement &z, const Eigen::Matrix< double, traits< Measurement >::dimension, traits< Measurement >::dimension > &R, bool performReset=true)
	Update overloads follow ManifoldEKF.
Public Member Functions inherited from gtsam::ManifoldEKF< G >
	ManifoldEKF (const G &X0, const Covariance &P0)
	Constructor: initialize with state and covariance.
const G &	state () const
const Covariance &	covariance () const
size_t	dimension () const
void	predict (const G &X_next, const Jacobian &F, const Covariance &Q)
	Basic predict step: Updates state and covariance given the predicted next state and the state transition Jacobian F.
auto	KalmanGain (const HMatrix &H, const RMatrix &R) const
	Kalman gain K = P H^T S^-1.
void	JosephUpdate (const GainMatrix &K, const HMatrix &H, const RMatrix &R)
	Joseph-form covariance update in the current tangent space using a precomputed gain.
void	update (const Measurement &prediction, const Eigen::Matrix< double, traits< Measurement >::dimension, Dim > &H, const Measurement &z, const Eigen::Matrix< double, traits< Measurement >::dimension, traits< Measurement >::dimension > &R, bool performReset=true)
	Measurement update: Corrects the state and covariance using a pre-calculated predicted measurement and its Jacobian.
void	update (MeasurementFunction &&h, const Measurement &z, const Eigen::Matrix< double, traits< Measurement >::dimension, traits< Measurement >::dimension > &R, bool performReset=true)
	Measurement update: Corrects the state and covariance using a measurement model function.
void	updateWithVector (const gtsam::Vector &prediction, const Matrix &H, const gtsam::Vector &z, const Matrix &R, bool performReset=true)
	Convenience bridge for wrappers: vector measurement update calling update<Vector>.
void	reset (const TangentVector &eta)
	Reset step: retract the state by a tangent perturbation and, if available, transport the covariance from the old tangent space to the new tangent space.

Static Public Member Functions
static NavState	Gravity (const Vector3 &g_n, double dt)
	Calculate W (gravity-only left composition, world-frame increments).
static NavState	Imu (const Vector3 &omega_b, const Vector3 &f_b, double dt)
	Calculate U from raw IMU (no gravity): body-frame increments We do an explicit closed-form integration based on SO(3) kernels.
static NavState	Dynamics (const Vector3 &g_n, const NavState &X, const Vector3 &omega_b, const Vector3 &f_b, double dt, OptionalJacobian< 9, 9 > A={})
	Compute the dynamics of the system.
Static Public Member Functions inherited from gtsam::LeftLinearEKF< NavState >
static NavState	Dynamics (const NavState &W, const Phi &phi, const NavState &X, const NavState &U, OptionalJacobian< Dim, Dim > A={})
	General left–linear dynamics.
static NavState	Dynamics (const Phi &phi, const NavState &X, const NavState &U, OptionalJacobian< Dim, Dim > A={})
	Left–linear dynamics with W=I.

Public Types
using	Base = LeftLinearEKF<NavState>
using	TangentVector = typename Base::TangentVector
using	Jacobian = typename Base::Jacobian
using	Covariance = typename Base::Covariance
Public Types inherited from gtsam::LeftLinearEKF< NavState >
using	Base
using	TangentVector
using	Jacobian
using	Covariance
Public Types inherited from gtsam::LieGroupEKF< G >
using	This = LieGroupEKF<G>
using	Base = ManifoldEKF<G>
	Base class type.
using	Jacobian = typename Base::Jacobian
	Dim x Dim.
using	Covariance = typename Base::Covariance
	Dim x Dim.
using	TangentVector = typename Base::TangentVector
	Tangent vector type.
Public Types inherited from gtsam::ManifoldEKF< G >
using	TangentVector
	Tangent vector type for the manifold M.
using	Covariance
	Covariance matrix type (P, Q).
using	Jacobian
	State transition Jacobian type (F).

Additional Inherited Members
Static Public Attributes inherited from gtsam::LeftLinearEKF< NavState >
static constexpr int	Dim
	Compile-time dimension of G.
Static Public Attributes inherited from gtsam::LieGroupEKF< G >
static constexpr int	Dim = Base::Dim
	Compile-time dimension of G.
Static Public Attributes inherited from gtsam::ManifoldEKF< G >
static constexpr int	Dim
	Compile-time dimension of the manifold M.
Protected Member Functions inherited from gtsam::ManifoldEKF< G >
void	validateInputs (const gtsam::Vector &prediction, const Matrix &H, const gtsam::Vector &z, const Matrix &R)
	Validate inputs to update.
Static Protected Member Functions inherited from gtsam::ManifoldEKF< G >
static bool	isMatrixOfSize (const MatrixType &matrix, size_t rows, size_t cols)
	Check whether a matrix has the expected runtime dimensions.
Protected Attributes inherited from gtsam::ManifoldEKF< G >
G	X_
	Manifold state estimate.
Covariance	P_
	Covariance (Eigen::Matrix<double, Dim, Dim>).
Jacobian	I_
	Identity matrix sized to the state dimension.
size_t	n_
	Runtime tangent space dimension of M.

Constructor & Destructor Documentation

◆ NavStateImuEKF()

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

Construct with initial state/covariance and preintegration params (for gravity and IMU covariances).

Parameters

X0	Initial NavState.
P0	Initial covariance in tangent space at X0.
params	Preintegration parameters providing gravity and options.

Member Function Documentation

◆ Dynamics()

NavState gtsam::NavStateImuEKF::Dynamics	(	const Vector3 &	g_n,
		const NavState &	X,
		const Vector3 &	omega_b,
		const Vector3 &	f_b,
		double	dt,
		OptionalJacobian< 9, 9 >	A = {} )

static

Compute the dynamics of the system.

This function computes the next state of the system based on the current state, gravity, body angular velocity, body specific force, and time step. The dynamics are defined as: X_{k+1} = f(X_k; g, omega_b, f_b, dt) = W(g, dt) \phi_dt(X_k) U(omega_b, f_b, dt) where W, \phi, and U are the gravity, (autonomous) position update, and IMU increment functions, respectively.

Parameters

g_n	Gravity vector in the navigation frame.
X	Current NavState.
omega_b	Body angular velocity measurement (rad/s).
f_b	Body specific force measurement (m/s^2).
dt	Time step in seconds.
A	Optional Jacobian of the dynamics with respect to the state.

Returns: The next NavState after applying the dynamics.

◆ predict()

void gtsam::NavStateImuEKF::predict	(	const Vector3 &	omega_b,
		const Vector3 &	f_b,
		double	dt )

Predict the next state using gyro and accelerometer measurements.

This method updates the state of the system based on the provided body angular velocity (omega_b) and body specific force (f_b) measurements over a given time step.

Parameters

omega_b	Body angular velocity measurement (rad/s).
f_b	Body specific force measurement (m/s^2).
dt	Time step in seconds.

The documentation for this class was generated from the following files:

/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/navigation/NavStateImuEKF.h
/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/navigation/NavStateImuEKF.cpp

Detailed Description

Public Member Functions

Static Public Member Functions

Public Types

Additional Inherited Members

Constructor & Destructor Documentation

◆ NavStateImuEKF()

Member Function Documentation

◆ Dynamics()

◆ predict()