gtsam::abc_eqf_lib Namespace Reference

Classes
class	EqF
	Equivariant Filter (EqF) implementation. More...
struct	G
struct	Input
	Input struct for the Biased Attitude System. More...
struct	Measurement
	Measurement struct. More...
class	State
	State class representing the state of the Biased Attitude System. More...

Functions
Matrix	blockDiag (const Matrix &A, const Matrix &B)
	Create a block diagonal matrix from two matrices. More...
bool	checkNorm (const Vector3 &x, double tol=1e-3)
	Check if a vector is a unit vector. More...
bool	hasNaN (const Vector3 &vec)
	Check if vector contains NaN values. More...
template<size_t N>
Vector	lift (const State< N > &xi, const Input &u)
Matrix	numericalDifferential (std::function< Vector(const Vector &)> f, const Vector &x)
	Calculate numerical differential. More...
template<size_t N>
State< N >	operator* (const G< N > &X, const State< N > &xi)
template<size_t N>
Vector3	outputAction (const G< N > &X, const Unit3 &y, int idx)
Matrix	repBlock (const Matrix &A, int n)
	Repeat a block matrix n times along the diagonal. More...
template<size_t N>
Matrix	stateActionDiff (const State< N > &xi)
template<size_t N>
Input	velocityAction (const G< N > &X, const Input &u)

Function Documentation

blockDiag()

Matrix gtsam::abc_eqf_lib::blockDiag	(	const Matrix &	A,
		const Matrix &	B
	)

Create a block diagonal matrix from two matrices.

Creates a block diagonal matrix from input matrices.

Parameters

A	Matrix A
B	Matrix B

Returns

A single consolidated matrix with A in the top left and B in the bottom right

Definition at line 73 of file ABC.h.

checkNorm()

bool gtsam::abc_eqf_lib::checkNorm	(	const Vector3 &	x,
		double	tol
	)

Check if a vector is a unit vector.

Verifies if a vector has unit norm within tolerance.

Parameters

x	3d vector
tol	optional tolerance

Returns

Bool indicating that the vector norm is approximately 1

Definition at line 53 of file ABC.h.

hasNaN()

bool gtsam::abc_eqf_lib::hasNaN

(

const Vector3 &

vec

)

Check if vector contains NaN values.

Checks if the input vector has any NaNs.

Parameters

vec	A 3-D vector

Returns

true if present, false otherwise

Definition at line 62 of file ABC.h.

lift()

template<size_t N>

Vector gtsam::abc_eqf_lib::lift	(	const State< N > &	xi,
		const Input &	u
	)

Compute the lift of the system (Theorem 3.8, Equation 7)

Parameters

xi	State
u	Input

Returns

Lift vector

Maps system dynamics to the symmetry group

Parameters

xi	State
u	Input

Returns

Lifted input in Lie Algebra Uses Vector zero & Rot3 inverse, matrix functions

Definition at line 187 of file ABC_EQF.h.

numericalDifferential()

Matrix gtsam::abc_eqf_lib::numericalDifferential	(	std::function< Vector(const Vector &)>	f,
		const Vector &	x
	)

Calculate numerical differential.

Calculates the Jacobian matrix using central difference approximation.

Parameters

f	Vector function f
x	The point at which Jacobian is evaluated

Returns

Matrix containing numerical partial derivatives of f at x Uses Vector's size() and Zero(), Matrix's Zero() and col() methods

Definition at line 266 of file ABC_EQF.h.

operator*()

template<size_t N>

State< N > gtsam::abc_eqf_lib::operator*	(	const G< N > &	X,
		const State< N > &	xi
	)

Action of the symmetry group on the state space (Equation 4)

Parameters

X	Group element
xi	State

Returns

New state after group action

Implements group actions on the states

Parameters

X	A symmetry group element G consisting of the attitude, bias and the calibration components X.a -> Rotation matrix containing the attitude X.b -> A skew-symmetric matrix representing bias X.B -> A vector of Rotation matrices for the calibration components
xi	State object xi.R -> Attitude (Rot3) xi.b -> Gyroscope Bias(Vector 3) xi.S -> Vector of calibration matrices(Rot3)

Returns

Transformed state Uses the Rot3 inverse and Vee functions

Definition at line 217 of file ABC_EQF.h.

outputAction()

template<size_t N>

Vector3 gtsam::abc_eqf_lib::outputAction	(	const G< N > &	X,
		const Unit3 &	y,
		int	idx
	)

Action of the symmetry group on the output space (Equation 6)

Parameters

X	Group element
y	Direction measurement
idx	Calibration index

Returns

New direction after group action

Transforms the Direction measurements based on the calibration type ( Eqn 6)

Parameters

X	Group element X
y	Direction measurement y
idx	Calibration index

Returns

Transformed direction Uses Rot3 inverse, matric and Unit3 unitvector functions

Definition at line 248 of file ABC_EQF.h.

repBlock()

Matrix gtsam::abc_eqf_lib::repBlock	(	const Matrix &	A,
		int	n
	)

Repeat a block matrix n times along the diagonal.

Creates a block diagonal matrix by repeating a matrix 'n' times.

Parameters

A	A matrix
n	Number of times to be repeated

Returns

Block diag matrix with A repeated 'n' times

Definition at line 93 of file ABC.h.

stateActionDiff()

template<size_t N>

Matrix gtsam::abc_eqf_lib::stateActionDiff

(

const State< N > &

xi

)

Differential of the phi action at E = Id in local coordinates

Parameters

xi

State

Returns

Differential matrix

Computes the differential of a state action at the identity of the symmetry group

Parameters

xi	State object Xi representing the point at which to evaluate the differential

Returns

A matrix representing the jacobian of the state action Uses numericalDifferential, and Rot3 expmap, logmap

Definition at line 296 of file ABC_EQF.h.

velocityAction()

template<size_t N>

Input gtsam::abc_eqf_lib::velocityAction	(	const G< N > &	X,
		const Input &	u
	)

Action of the symmetry group on the input space (Equation 5)

Parameters

X	Group element
u	Input

Returns

New input after group action

Transforms the angular velocity measurements b/w frames

Parameters

X	A symmetry group element X with the components
u	Inputs

Returns

Transformed inputs Uses Rot3 Inverse, matrix and Vee functions and is critical for maintaining the input equivariance

Definition at line 236 of file ABC_EQF.h.