gtsam::abc_eqf_lib::EqF< N > Class Template Reference

Equivariant Filter (EqF) implementation. More...

#include <ABC_EQF.h>

Public Member Functions
	EqF (const Matrix &Sigma, int m)
void	propagation (const Input &u, double dt)
State< N >	stateEstimate () const
void	update (const Measurement &y)

Private Member Functions
Matrix	inputMatrixBt () const
Matrix	measurementMatrixC (const Unit3 &d, int idx) const
Matrix	outputMatrixDt (int idx) const
Matrix	stateMatrixA (const Input &u) const
Matrix	stateTransitionMatrix (const Input &u, double dt) const

Private Attributes
int	dof
Matrix	Dphi0
Matrix	InnovationLift
Matrix	Sigma
G< N >	X_hat
State< N >	xi_0

Detailed Description

template<size_t N>
class gtsam::abc_eqf_lib::EqF< N >

Equivariant Filter (EqF) implementation.

Definition at line 102 of file ABC_EQF.h.

Constructor & Destructor Documentation

EqF()

template<size_t N>

gtsam::abc_eqf_lib::EqF< N >::EqF	(	const Matrix &	Sigma,
		int	m
	)

Initialize EqF

Parameters

Sigma	Initial covariance
m	Number of sensors

Initializes the EqF with state dimension validation and computes lifted innovation mapping

Parameters

Sigma	Initial covariance
n	Number of calibration states
m	Number of sensors Uses SelfAdjointSolver, completeOrthoganalDecomposition().pseudoInverse()

Definition at line 320 of file ABC_EQF.h.

Member Function Documentation

inputMatrixBt()

template<size_t N>

Matrix gtsam::abc_eqf_lib::EqF< N >::inputMatrixBt

private

Return the Input matrix Bt

Returns

Input matrix Bt

Computes the input uncertainty propagation matrix

Returns

Uses the blockdiag matrix

Definition at line 456 of file ABC_EQF.h.

measurementMatrixC()

template<size_t N>

Matrix gtsam::abc_eqf_lib::EqF< N >::measurementMatrixC ( const Unit3 &  d, int  idx  ) const

private

Return the measurement matrix C0 (Equation 14b)

Parameters

d	Known direction
idx	Calibration index

Returns

Measurement matrix C0

Computes the linearized measurement matrix. The structure depends on whether the sensor has a calibration state

Parameters

d	reference direction
idx	Calibration index

Returns

Measurement matrix Uses the matrix zero, Rot3 hat and the Unitvector functions

Definition at line 475 of file ABC_EQF.h.

outputMatrixDt()

template<size_t N>

Matrix gtsam::abc_eqf_lib::EqF< N >::outputMatrixDt ( int  idx ) const

private

Return the measurement output matrix Dt

Parameters

idx	Calibration index

Returns

Measurement output matrix Dt

Computes the measurement uncertainty propagation matrix

Parameters

idx	Calibration index

Returns

Returns B[idx] for calibrated sensors, A for uncalibrated

Definition at line 500 of file ABC_EQF.h.

propagation()

template<size_t N>

void gtsam::abc_eqf_lib::EqF< N >::propagation	(	const Input &	u,
		double	dt
	)

Propagate the filter state

Parameters

u	Angular velocity measurement
dt	Time step

Implements the prediction step of the EqF using system dynamics and covariance propagation and advances the filter state by symmtery-preserving dynamics.Uses a Lie group integrator scheme for discrete time propagation

Parameters

u	Angular velocity measurements
dt	time steps Updated internal state and covariance

Definition at line 368 of file ABC_EQF.h.

stateEstimate()

template<size_t N>

State< N > gtsam::abc_eqf_lib::EqF< N >::stateEstimate

Return estimated state

Returns

Current state estimate

Computes the internal group state to a physical state estimate

Returns

Current state estimate

Definition at line 356 of file ABC_EQF.h.

stateMatrixA()

template<size_t N>

Matrix gtsam::abc_eqf_lib::EqF< N >::stateMatrixA ( const Input &  u ) const

private

Return the state matrix A0t (Equation 14a)

Parameters

u

Input

Returns

State matrix A0t

Computes linearized continuous time state matrix

Parameters

u	Angular velocity

Returns

Linearized state matrix Uses Matrix zero and Identity functions

Definition at line 421 of file ABC_EQF.h.

stateTransitionMatrix()

template<size_t N>

Matrix gtsam::abc_eqf_lib::EqF< N >::stateTransitionMatrix ( const Input &  u, double  dt  ) const

private

Return the state transition matrix Phi (Equation 17)

Parameters

u	Input
dt	Time step

Returns

State transition matrix Phi

Computes the discrete time state transition matrix

Parameters

u	Angular velocity
dt	time step

Returns

State transition matrix in discrete time

Definition at line 437 of file ABC_EQF.h.

update()

template<size_t N>

void gtsam::abc_eqf_lib::EqF< N >::update

(

const Measurement &

y

)

Update the filter state with a measurement

Parameters

y	Direction measurement

Implements the correction step of the filter using discrete measurements Computes the measurement residual, Kalman gain and the updates both the state and covariance

Parameters

y	Measurements

Definition at line 389 of file ABC_EQF.h.

Member Data Documentation

dof

template<size_t N>

int gtsam::abc_eqf_lib::EqF< N >::dof

private

Definition at line 104 of file ABC_EQF.h.

Dphi0

template<size_t N>

Matrix gtsam::abc_eqf_lib::EqF< N >::Dphi0

private

Definition at line 108 of file ABC_EQF.h.

InnovationLift

template<size_t N>

Matrix gtsam::abc_eqf_lib::EqF< N >::InnovationLift

private

Definition at line 109 of file ABC_EQF.h.

Sigma

template<size_t N>

Matrix gtsam::abc_eqf_lib::EqF< N >::Sigma

private

Definition at line 106 of file ABC_EQF.h.

X_hat

template<size_t N>

G<N> gtsam::abc_eqf_lib::EqF< N >::X_hat

private

Definition at line 105 of file ABC_EQF.h.

xi_0

template<size_t N>

State<N> gtsam::abc_eqf_lib::EqF< N >::xi_0

private

Definition at line 107 of file ABC_EQF.h.

---

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

• ABC_EQF.h