gtsam::abc::AbcEquivariantFilter< N > Class Template Reference

Detailed Description

template<size_t N>
class gtsam::abc::AbcEquivariantFilter< N >

Equivariant Filter for Attitude-Bias-Calibration (ABC) estimation.

This class implements an equivariant filter for estimating:

• Attitude (rotation): The orientation of the body frame relative to a reference frame
• Bias: Gyroscope bias correction vector (3D)
• Calibration: N sensor calibration rotation matrices

The filter uses the ABC Lie group structure and equivariant dynamics to provide consistent state estimation. It inherits from EquivariantFilter and provides a simplified interface for ABC-specific operations.

Template Parameters

N	Number of calibrated sensors (typically 1 or more)

Inheritance diagram for gtsam::abc::AbcEquivariantFilter< N >:

Public Member Functions
	AbcEquivariantFilter ()
	Default constructor with identity initial covariance.
	AbcEquivariantFilter (const Matrix &Sigma0)
	Construct filter with custom initial covariance.
void	predict (const Vector3 &omega, const Matrix6 &inputCovariance, double dt)
	Prediction step using gyroscope measurements.
void	update (const Unit3 &y, const Unit3 &d, const Matrix3 &R, int cal_idx)
	Measurement update using a direction observation.
Rot3	attitude () const
	Get current attitude estimate.
Vector3	bias () const
	Get current gyroscope bias estimate.
Rot3	calibration (size_t i) const
	Get calibration estimate for a specific sensor.
Public Member Functions inherited from gtsam::EquivariantFilter< State< N >, Symmetry< N > >
const State< N > &	state () const
	State on the manifold M is given by the base class.
	EquivariantFilter (const State< N > &xi_ref, const CovarianceM &Sigma, const G &X0=traits< G >::Identity())
	Initialize the Equivariant Filter.
const Base::Covariance &	errorCovariance () const
	errorCovariance that returns P_, on the equivariant filter error
MatrixM	actionDifferential () const
	Differential of the group action at the reference state.
CovarianceM	covariance () const
	Covariance in the tangent space at the current state.
const G &	groupEstimate () const
MatrixM	computeErrorDynamicsMatrix (const InputOrbit &psi_u) const
	Compute the error dynamics matrix A (Automatic).
MatrixM	transitionMatrix (const MatrixM &A, double dt) const
	Discretize continuous-time error dynamics δ̇ = A δ over dt.
void	predict (const Lift &lift_u, const InputOrbit &psi_u, const MatrixM &Qc, double dt)
	Propagate the filter state (Automatic).
void	predictWithJacobian (const Lift &lift_u, const MatrixM &A, const MatrixM &Qc, double dt)
	Propagate the filter state (Explicit).
void	predictWithTransition (const Lift &lift_u, const MatrixM &Phi, const CovarianceM &Qd, double dt)
	Propagate with an already-discretized transition and process noise.
void	update (const Measurement &prediction, const Eigen::Matrix< double, traits< Measurement >::dimension, DimM > &H, const Measurement &z, const Eigen::Matrix< double, traits< Measurement >::dimension, traits< Measurement >::dimension > &R)
	Measurement update: Corrects the state and covariance using a pre-calculated predicted measurement and its Jacobian.
void	updateWithVector (const gtsam::Vector &prediction, const Matrix &H, const gtsam::Vector &z, const Matrix &R)
	Same API as ManifoldEKF for measurement update with vector inputs.
Public Member Functions inherited from gtsam::ManifoldEKF< State< N > >
	ManifoldEKF (const State< N > &X0, const Covariance &P0)
	Constructor: initialize with state and covariance.
const State< N > &	state () const
const Covariance &	covariance () const
size_t	dimension () const
void	predict (const State< N > &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	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.

Additional Inherited Members
Static Public Attributes inherited from gtsam::EquivariantFilter< State< N >, Symmetry< N > >
static constexpr int	DimM
static constexpr int	DimG
Static Public Attributes inherited from gtsam::ManifoldEKF< State< N > >
static constexpr int	Dim
	Compile-time dimension of the manifold M.
Public Types inherited from gtsam::EquivariantFilter< State< N >, Symmetry< N > >
using	Base
using	TangentM
using	MatrixM
using	CovarianceM
using	G
using	TangentG
using	MatrixMG
using	MatrixGM
Public Types inherited from gtsam::ManifoldEKF< State< N > >
using	TangentVector
	Tangent vector type for the manifold M.
using	Covariance
	Covariance matrix type (P, Q).
using	Jacobian
	State transition Jacobian type (F).
Protected Member Functions inherited from gtsam::EquivariantFilter< State< N >, Symmetry< N > >
void	resetReferenceAndGroup (const State< N > &xi_ref, const CovarianceM &P, const G &g)
	Synchronization hooks for derived filters with dynamic runtime structure.
const State< N > &	referenceState () const
	Access current reference state used as the EqF chart origin.
TangentG	liftAtOrigin (const InputOrbit &psi_u, OptionalJacobian< DimG, DimM > D_lift={}) const
	Evaluate the lift at the reference with input u_origin = psi_u(X^-1).
void	propagate (const TangentG &increment, const MatrixM &Phi, const CovarianceM &Qd)
	Advance using a lift evaluated at the current estimate and propagate covariance with an already-discretized transition and process noise.
void	applyCorrection (const TangentG &delta_x)
	Apply an innovation correction, which lives in error coordinates at the reference state, on the opposite side from the prediction increment.
Protected Member Functions inherited from gtsam::ManifoldEKF< State< N > >
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< State< N > >
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< State< N > >
State< N >	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

AbcEquivariantFilter() [1/2]

template<size_t N>

gtsam::abc::AbcEquivariantFilter< N >::AbcEquivariantFilter ( )

inline

Default constructor with identity initial covariance.

Initializes the filter with an identity state and identity covariance matrix of dimension (6 + 3*N) x (6 + 3*N), where 6 accounts for the attitude and bias parameters, and 3*N for N calibration parameters.

AbcEquivariantFilter() [2/2]

template<size_t N>

gtsam::abc::AbcEquivariantFilter< N >::AbcEquivariantFilter ( const Matrix & Sigma0 )

inlineexplicit

Construct filter with custom initial covariance.

Initializes the filter at the identity state with a specified initial covariance matrix on the manifold tangent space.

Parameters

Sigma0

Initial covariance matrix (6+3*N) x (6+3*N)

Member Function Documentation

attitude()

template<size_t N>

Rot3 gtsam::abc::AbcEquivariantFilter< N >::attitude ( ) const

inline

Get current attitude estimate.

Returns

Current rotation estimate (body frame to reference frame)

bias()

template<size_t N>

Vector3 gtsam::abc::AbcEquivariantFilter< N >::bias ( ) const

inline

Get current gyroscope bias estimate.

Returns

Current bias vector (rad/s)

calibration()

template<size_t N>

Rot3 gtsam::abc::AbcEquivariantFilter< N >::calibration ( size_t i ) const

inline

Get calibration estimate for a specific sensor.

Parameters

i	Sensor index (0 to N-1)

Returns

Calibration rotation for sensor i

predict()

template<size_t N>

void gtsam::abc::AbcEquivariantFilter< N >::predict ( const Vector3 & omega, const Matrix6 & inputCovariance, double dt )

inline

Prediction step using gyroscope measurements.

Propagates the filter state forward in time using angular velocity measurements and process noise. This method uses the explicit Jacobian matrices (A and B) computed from the ABC dynamics.

Parameters

omega	Angular velocity measurement (body frame, rad/s)
inputCovariance	Process noise covariance (6x6) for the input
dt	Time step (seconds)

update()

template<size_t N>

void gtsam::abc::AbcEquivariantFilter< N >::update ( const Unit3 & y, const Unit3 & d, const Matrix3 & R, int cal_idx )

inline

Measurement update using a direction observation.

Corrects the filter estimate using a direction measurement from a calibrated sensor. The measurement model assumes the sensor observes a known reference direction in the body frame.

Parameters

y	Measured direction (unit vector in body frame)
d	Reference direction (unit vector in reference frame)
R	Measurement noise covariance (3x3)
cal_idx	Index of the calibrated sensor (0 to N-1), or -1 for uncalibrated measurements

---

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

• /tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam_unstable/geometry/ABCEquivariantFilter.h