#include <NavState.h>

Public Types
enum	{ dimension = 9 }

typedef std::pair< Point3, Velocity3 >	PositionAndVelocity

Public Member Functions
Component Access
const Rot3 &	attitude (OptionalJacobian< 3, 9 > H={}) const

const Point3 &	position (OptionalJacobian< 3, 9 > H={}) const

const Velocity3 &	velocity (OptionalJacobian< 3, 9 > H={}) const

const Pose3	pose () const

Derived quantities
Matrix3	R () const
	Return rotation matrix. Induces computation in quaternion mode. More...

Quaternion	quaternion () const
	Return quaternion. Induces computation in matrix mode. More...

Vector3	t () const
	Return position as Vector3. More...

const Vector3 &	v () const
	Return velocity as Vector3. Computation-free. More...

Velocity3	bodyVelocity (OptionalJacobian< 3, 9 > H={}) const

Matrix7	matrix () const

Dynamics
NavState	update (const Vector3 &b_acceleration, const Vector3 &b_omega, const double dt, OptionalJacobian< 9, 9 > F, OptionalJacobian< 9, 3 > G1, OptionalJacobian< 9, 3 > G2) const

Vector9	coriolis (double dt, const Vector3 &omega, bool secondOrder=false, OptionalJacobian< 9, 9 > H={}) const
	Compute tangent space contribution due to Coriolis forces. More...

Vector9	correctPIM (const Vector9 &pim, double dt, const Vector3 &n_gravity, const std::optional< Vector3 > &omegaCoriolis, bool use2ndOrderCoriolis=false, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const

Private Attributes
Rot3	R_
	Rotation nRb, rotates points/velocities in body to points/velocities in nav. More...

Point3	t_
	position n_t, in nav frame More...

Velocity3	v_
	velocity n_v in nav frame More...

Constructors
	NavState ()
	Default constructor. More...

	NavState (const Rot3 &R, const Point3 &t, const Velocity3 &v)
	Construct from attitude, position, velocity. More...

	NavState (const Pose3 &pose, const Velocity3 &v)
	Construct from pose and velocity. More...

	NavState (const Matrix3 &R, const Vector6 &tv)
	Construct from SO(3) and R^6. More...

static NavState	Create (const Rot3 &R, const Point3 &t, const Velocity3 &v, OptionalJacobian< 9, 3 > H1, OptionalJacobian< 9, 3 > H2, OptionalJacobian< 9, 3 > H3)
	Named constructor with derivatives. More...

static NavState	FromPoseVelocity (const Pose3 &pose, const Vector3 &vel, OptionalJacobian< 9, 6 > H1, OptionalJacobian< 9, 3 > H2)
	Named constructor with derivatives. More...

Testable
void	print (const std::string &s="") const
	print More...

bool	equals (const NavState &other, double tol=1e-8) const
	equals More...

GTSAM_EXPORT friend std::ostream &	operator<< (std::ostream &os, const NavState &state)
	Output stream operator. More...

Manifold
NavState	retract (const Vector9 &v, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
	retract with optional derivatives More...

Vector9	localCoordinates (const NavState &g, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
	localCoordinates with optional derivatives More...

static Eigen::Block< Vector9, 3, 1 >	dR (Vector9 &v)

static Eigen::Block< Vector9, 3, 1 >	dP (Vector9 &v)

static Eigen::Block< Vector9, 3, 1 >	dV (Vector9 &v)

static Eigen::Block< const Vector9, 3, 1 >	dR (const Vector9 &v)

static Eigen::Block< const Vector9, 3, 1 >	dP (const Vector9 &v)

static Eigen::Block< const Vector9, 3, 1 >	dV (const Vector9 &v)

Detailed Description

Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make this a Lie group, but it is a 9D manifold

Definition at line 34 of file NavState.h.

Member Typedef Documentation

◆ PositionAndVelocity

typedef std::pair<Point3, Velocity3> gtsam::NavState::PositionAndVelocity

Definition at line 49 of file NavState.h.

Member Enumeration Documentation

◆ anonymous enum

anonymous enum

Enumerator
dimension

Definition at line 45 of file NavState.h.

Constructor & Destructor Documentation

◆ NavState() [1/4]

gtsam::NavState::NavState ( )

inline

Default constructor.

Definition at line 55 of file NavState.h.

◆ NavState() [2/4]

gtsam::NavState::NavState	(	const Rot3 &	R,
		const Point3 &	t,
		const Velocity3 &	v
	)

inline

Construct from attitude, position, velocity.

Definition at line 59 of file NavState.h.

◆ NavState() [3/4]

gtsam::NavState::NavState	(	const Pose3 &	pose,
		const Velocity3 &	v
	)

inline

Construct from pose and velocity.

Definition at line 63 of file NavState.h.

◆ NavState() [4/4]

gtsam::NavState::NavState	(	const Matrix3 &	R,
		const Vector6 &	tv
	)

inline

Construct from SO(3) and R^6.

Definition at line 67 of file NavState.h.

Member Function Documentation

◆ attitude()

const Rot3 & gtsam::NavState::attitude ( OptionalJacobian< 3, 9 > H = {} ) const

Definition at line 48 of file NavState.cpp.

◆ bodyVelocity()

Vector3 gtsam::NavState::bodyVelocity ( OptionalJacobian< 3, 9 > H = {} ) const

Definition at line 69 of file NavState.cpp.

◆ coriolis()

Vector9 gtsam::NavState::coriolis	(	double	dt,
		const Vector3 &	omega,
		bool	secondOrder = `false`,
		OptionalJacobian< 9, 9 >	H = `{}`
	)		const

Compute tangent space contribution due to Coriolis forces.

Definition at line 213 of file NavState.cpp.

◆ correctPIM()

Vector9 gtsam::NavState::correctPIM	(	const Vector9 &	pim,
		double	dt,
		const Vector3 &	n_gravity,
		const std::optional< Vector3 > &	omegaCoriolis,
		bool	use2ndOrderCoriolis = `false`,
		OptionalJacobian< 9, 9 >	H1 = `{}`,
		OptionalJacobian< 9, 9 >	H2 = `{}`
	)		const

Correct preintegrated tangent vector with our velocity and rotated gravity, taking into account Coriolis forces if omegaCoriolis is given.

Definition at line 257 of file NavState.cpp.

◆ Create()

NavState gtsam::NavState::Create	(	const Rot3 &	R,
		const Point3 &	t,
		const Velocity3 &	v,
		OptionalJacobian< 9, 3 >	H1,
		OptionalJacobian< 9, 3 >	H2,
		OptionalJacobian< 9, 3 >	H3
	)

static

Named constructor with derivatives.

Definition at line 26 of file NavState.cpp.

◆ dP() [1/2]

static Eigen::Block<Vector9, 3, 1> gtsam::NavState::dP ( Vector9 & v )

inlinestatic

Definition at line 141 of file NavState.h.

◆ dP() [2/2]

static Eigen::Block<const Vector9, 3, 1> gtsam::NavState::dP ( const Vector9 & v )

inlinestatic

Definition at line 150 of file NavState.h.

◆ dR() [1/2]

static Eigen::Block<Vector9, 3, 1> gtsam::NavState::dR ( Vector9 & v )

inlinestatic

Definition at line 138 of file NavState.h.

◆ dR() [2/2]

static Eigen::Block<const Vector9, 3, 1> gtsam::NavState::dR ( const Vector9 & v )

inlinestatic

Definition at line 147 of file NavState.h.

◆ dV() [1/2]

static Eigen::Block<Vector9, 3, 1> gtsam::NavState::dV ( Vector9 & v )

inlinestatic

Definition at line 144 of file NavState.h.

◆ dV() [2/2]

static Eigen::Block<const Vector9, 3, 1> gtsam::NavState::dV ( const Vector9 & v )

inlinestatic

Definition at line 153 of file NavState.h.

◆ equals()

bool gtsam::NavState::equals	(	const NavState &	other,
		double	tol = `1e-8`
	)		const

equals

Definition at line 101 of file NavState.cpp.

◆ FromPoseVelocity()

NavState gtsam::NavState::FromPoseVelocity	(	const Pose3 &	pose,
		const Vector3 &	vel,
		OptionalJacobian< 9, 6 >	H1,
		OptionalJacobian< 9, 3 >	H2
	)

static

Named constructor with derivatives.

Definition at line 38 of file NavState.cpp.

◆ localCoordinates()

Vector9 gtsam::NavState::localCoordinates	(	const NavState &	g,
		OptionalJacobian< 9, 9 >	H1 = `{}`,
		OptionalJacobian< 9, 9 >	H2 = `{}`
	)		const

localCoordinates with optional derivatives

Definition at line 133 of file NavState.cpp.

◆ matrix()

Matrix7 gtsam::NavState::matrix ( ) const

Return matrix group representation, in MATLAB notation: nTb = [nRb 0 n_t; 0 nRb n_v; 0 0 1] With this embedding in GL(3), matrix product agrees with compose

Definition at line 80 of file NavState.cpp.

◆ pose()

const Pose3 gtsam::NavState::pose ( ) const

inline

Definition at line 86 of file NavState.h.

◆ position()

const Point3 & gtsam::NavState::position ( OptionalJacobian< 3, 9 > H = {} ) const

Definition at line 55 of file NavState.cpp.

◆ print()

void gtsam::NavState::print ( const std::string & s = "" ) const

print

Definition at line 96 of file NavState.cpp.

◆ quaternion()

Quaternion gtsam::NavState::quaternion ( ) const

inline

Return quaternion. Induces computation in matrix mode.

Definition at line 99 of file NavState.h.

◆ R()

Matrix3 gtsam::NavState::R ( ) const

inline

Return rotation matrix. Induces computation in quaternion mode.

Definition at line 95 of file NavState.h.

◆ retract()

NavState gtsam::NavState::retract	(	const Vector9 &	v,
		OptionalJacobian< 9, 9 >	H1 = `{}`,
		OptionalJacobian< 9, 9 >	H2 = `{}`
	)		const

retract with optional derivatives

Definition at line 107 of file NavState.cpp.

◆ t()

Vector3 gtsam::NavState::t ( ) const

inline

Return position as Vector3.

Definition at line 103 of file NavState.h.

◆ update()

NavState gtsam::NavState::update	(	const Vector3 &	b_acceleration,
		const Vector3 &	b_omega,
		const double	dt,
		OptionalJacobian< 9, 9 >	F,
		OptionalJacobian< 9, 3 >	G1,
		OptionalJacobian< 9, 3 >	G2
	)		const

Integrate forward in time given angular velocity and acceleration in body frame Uses second order integration for position, returns derivatives except dt.

Definition at line 169 of file NavState.cpp.

◆ v()

const Vector3& gtsam::NavState::v ( ) const

inline

Return velocity as Vector3. Computation-free.

Definition at line 107 of file NavState.h.

◆ velocity()

const Vector3 & gtsam::NavState::velocity ( OptionalJacobian< 3, 9 > H = {} ) const

Definition at line 62 of file NavState.cpp.

Friends And Related Function Documentation

◆ operator<<

GTSAM_EXPORT friend std::ostream& operator<<	(	std::ostream &	os,
		const NavState &	state
	)

friend

Output stream operator.

Definition at line 88 of file NavState.cpp.

Member Data Documentation

◆ R_

Rot3 gtsam::NavState::R_

private

Rotation nRb, rotates points/velocities in body to points/velocities in nav.

Definition at line 39 of file NavState.h.

◆ t_

Point3 gtsam::NavState::t_

private

position n_t, in nav frame

Definition at line 40 of file NavState.h.

◆ v_

Velocity3 gtsam::NavState::v_

private

velocity n_v in nav frame

Definition at line 41 of file NavState.h.

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

Public Types

Public Member Functions

Private Attributes

Constructors

Testable

Manifold

Detailed Description

Member Typedef Documentation

◆ PositionAndVelocity

Member Enumeration Documentation

◆ anonymous enum

Constructor & Destructor Documentation

◆ NavState() [1/4]

◆ NavState() [2/4]

◆ NavState() [3/4]

◆ NavState() [4/4]

Member Function Documentation

◆ attitude()

◆ bodyVelocity()

◆ coriolis()

◆ correctPIM()

◆ Create()

◆ dP() [1/2]

◆ dP() [2/2]

◆ dR() [1/2]

◆ dR() [2/2]

◆ dV() [1/2]

◆ dV() [2/2]

◆ equals()

◆ FromPoseVelocity()

◆ localCoordinates()

◆ matrix()

◆ pose()

◆ position()

◆ print()

◆ quaternion()

◆ R()

◆ retract()

◆ t()

◆ update()

◆ v()

◆ velocity()

Friends And Related Function Documentation

◆ operator<<

Member Data Documentation

◆ R_

◆ t_

◆ v_