12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #2103 from jenniferoum/feature/equivariant-filter 

C++ example of an Equivariant filter
release/4.3a0
Frank Dellaert 2025-04-26 00:16:41 -04:00 committed by GitHub
parent 3eac859bf9 17bf752576
commit deb4045aa6
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
4 changed files with 1223 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

259
examples/ABC.h Normal file
View File

@ -0,0 +1,259 @@
/**
* @file ABC.h
* @brief Core components for Attitude-Bias-Calibration systems
*
* This file contains fundamental components and utilities for the ABC system
* based on the paper "Overcoming Bias: Equivariant Filter Design for Biased
* Attitude Estimation with Online Calibration" by Fornasier et al.
* Authors: Darshan Rajasekaran & Jennifer Oum
*/
#ifndef ABC_H
#define ABC_H
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Unit3.h>
namespace gtsam {
namespace abc_eqf_lib {
using namespace std;
using namespace gtsam;
//========================================================================
// Utility Functions
//========================================================================
//========================================================================
// Utility Functions
//========================================================================
/// Check if a vector is a unit vector
bool checkNorm(const Vector3& x, double tol = 1e-3);
/// Check if vector contains NaN values
bool hasNaN(const Vector3& vec);
/// Create a block diagonal matrix from two matrices
Matrix blockDiag(const Matrix& A, const Matrix& B);
/// Repeat a block matrix n times along the diagonal
Matrix repBlock(const Matrix& A, int n);
// Utility Functions Implementation
/**
* @brief Verifies if a vector has unit norm within tolerance
* @param x 3d vector
* @param tol optional tolerance
* @return Bool indicating that the vector norm is approximately 1
*/
bool checkNorm(const Vector3& x, double tol) {
return abs(x.norm() - 1) < tol || std::isnan(x.norm());
}
/**
* @brief Checks if the input vector has any NaNs
* @param vec A 3-D vector
* @return true if present, false otherwise
*/
bool hasNaN(const Vector3& vec) {
return std::isnan(vec[0]) || std::isnan(vec[1]) || std::isnan(vec[2]);
}
/**
* @brief Creates a block diagonal matrix from input matrices
* @param A Matrix A
* @param B Matrix B
* @return A single consolidated matrix with A in the top left and B in the
* bottom right
*/
Matrix blockDiag(const Matrix& A, const Matrix& B) {
if (A.size() == 0) {
return B;
} else if (B.size() == 0) {
return A;
} else {
Matrix result(A.rows() + B.rows(), A.cols() + B.cols());
result.setZero();
result.block(0, 0, A.rows(), A.cols()) = A;
result.block(A.rows(), A.cols(), B.rows(), B.cols()) = B;
return result;
}
}
/**
* @brief Creates a block diagonal matrix by repeating a matrix 'n' times
* @param A A matrix
* @param n Number of times to be repeated
* @return Block diag matrix with A repeated 'n' times
*/
Matrix repBlock(const Matrix& A, int n) {
if (n <= 0) return Matrix();
Matrix result = A;
for (int i = 1; i < n; i++) {
result = blockDiag(result, A);
}
return result;
}
//========================================================================
// Core Data Types
//========================================================================
/// Input struct for the Biased Attitude System
struct Input {
Vector3 w; /// Angular velocity (3-vector)
Matrix Sigma; /// Noise covariance (6x6 matrix)
static Input random(); /// Random input
Matrix3 W() const { /// Return w as a skew symmetric matrix
return Rot3::Hat(w);
}
};
/// Measurement struct
struct Measurement {
Unit3 y; /// Measurement direction in sensor frame
Unit3 d; /// Known direction in global frame
Matrix3 Sigma; /// Covariance matrix of the measurement
int cal_idx = -1; /// Calibration index (-1 for calibrated sensor)
};
/// State class representing the state of the Biased Attitude System
template <size_t N>
class State {
public:
Rot3 R; // Attitude rotation matrix R
Vector3 b; // Gyroscope bias b
std::array<Rot3, N> S; // Sensor calibrations S
/// Constructor
State(const Rot3& R = Rot3::Identity(), const Vector3& b = Vector3::Zero(),
const std::array<Rot3, N>& S = std::array<Rot3, N>{})
: R(R), b(b), S(S) {}
/// Identity function
static State identity() {
std::array<Rot3, N> S_id{};
S_id.fill(Rot3::Identity());
return State(Rot3::Identity(), Vector3::Zero(), S_id);
}
/**
* Compute Local coordinates in the state relative to another state.
* @param other The other state
* @return Local coordinates in the tangent space
*/
Vector localCoordinates(const State<N>& other) const {
Vector eps(6 + 3 * N);
// First 3 elements - attitude
eps.head<3>() = Rot3::Logmap(R.between(other.R));
// Next 3 elements - bias
// Next 3 elements - bias
eps.segment<3>(3) = other.b - b;
// Remaining elements - calibrations
for (size_t i = 0; i < N; i++) {
eps.segment<3>(6 + 3 * i) = Rot3::Logmap(S[i].between(other.S[i]));
}
return eps;
}
/**
* Retract from tangent space back to the manifold
* @param v Vector in the tangent space
* @return New state
*/
State retract(const Vector& v) const {
if (v.size() != static_cast<Eigen::Index>(6 + 3 * N)) {
throw std::invalid_argument(
"Vector size does not match state dimensions");
}
Rot3 newR = R * Rot3::Expmap(v.head<3>());
Vector3 newB = b + v.segment<3>(3);
std::array<Rot3, N> newS;
for (size_t i = 0; i < N; i++) {
newS[i] = S[i] * Rot3::Expmap(v.segment<3>(6 + 3 * i));
}
return State(newR, newB, newS);
}
};
//========================================================================
// Symmetry Group
//========================================================================
/**
* Symmetry group (SO(3) |x so(3)) x SO(3) x ... x SO(3)
* Each element of the B list is associated with a calibration state
*/
template <size_t N>
struct G {
Rot3 A; /// First SO(3) element
Matrix3 a; /// so(3) element (skew-symmetric matrix)
std::array<Rot3, N> B; /// List of SO(3) elements for calibration
/// Initialize the symmetry group G
G(const Rot3& A = Rot3::Identity(), const Matrix3& a = Matrix3::Zero(),
const std::array<Rot3, N>& B = std::array<Rot3, N>{})
: A(A), a(a), B(B) {}
/// Group multiplication
G operator*(const G<N>& other) const {
std::array<Rot3, N> newB;
for (size_t i = 0; i < N; i++) {
newB[i] = B[i] * other.B[i];
}
return G(A * other.A, a + Rot3::Hat(A.matrix() * Rot3::Vee(other.a)), newB);
}
/// Group inverse
G inv() const {
Matrix3 Ainv = A.inverse().matrix();
std::array<Rot3, N> Binv;
for (size_t i = 0; i < N; i++) {
Binv[i] = B[i].inverse();
}
return G(A.inverse(), -Rot3::Hat(Ainv * Rot3::Vee(a)), Binv);
}
/// Identity element
static G identity(int n) {
std::array<Rot3, N> B;
B.fill(Rot3::Identity());
return G(Rot3::Identity(), Matrix3::Zero(), B);
}
/// Exponential map of the tangent space elements to the group
static G exp(const Vector& x) {
if (x.size() != static_cast<Eigen::Index>(6 + 3 * N)) {
throw std::invalid_argument("Vector size mismatch for group exponential");
}
Rot3 A = Rot3::Expmap(x.head<3>());
Vector3 a_vee = Rot3::ExpmapDerivative(-x.head<3>()) * x.segment<3>(3);
Matrix3 a = Rot3::Hat(a_vee);
std::array<Rot3, N> B;
for (size_t i = 0; i < N; i++) {
B[i] = Rot3::Expmap(x.segment<3>(6 + 3 * i));
}
return G(A, a, B);
}
};
} // namespace abc_eqf_lib
template <size_t N>
struct traits<abc_eqf_lib::State<N>>
: internal::LieGroupTraits<abc_eqf_lib::State<N>> {};
template <size_t N>
struct traits<abc_eqf_lib::G<N>> : internal::LieGroupTraits<abc_eqf_lib::G<N>> {
};
} // namespace gtsam
#endif // ABC_H

519
examples/ABC_EQF.h Normal file
View File

@ -0,0 +1,519 @@
/**
* @file ABC_EQF.h
* @brief Header file for the Attitude-Bias-Calibration Equivariant Filter
*
* This file contains declarations for the Equivariant Filter (EqF) for attitude
* estimation with both gyroscope bias and sensor extrinsic calibration, based
* on the paper: "Overcoming Bias: Equivariant Filter Design for Biased Attitude
* Estimation with Online Calibration" by Fornasier et al. Authors: Darshan
* Rajasekaran & Jennifer Oum
*/
#ifndef ABC_EQF_H
#define ABC_EQF_H
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/dataset.h>
#include <chrono>
#include <cmath>
#include <fstream>
#include <functional>
#include <iostream>
#include <numeric> // For std::accumulate
#include <string>
#include <vector>
#include "ABC.h"
// All implementations are wrapped in this namespace to avoid conflicts
namespace gtsam {
namespace abc_eqf_lib {
using namespace std;
using namespace gtsam;
//========================================================================
// Helper Functions for EqF
//========================================================================
/// Calculate numerical differential
Matrix numericalDifferential(std::function<Vector(const Vector&)> f,
const Vector& x);
/**
* Compute the lift of the system (Theorem 3.8, Equation 7)
* @param xi State
* @param u Input
* @return Lift vector
*/
template <size_t N>
Vector lift(const State<N>& xi, const Input& u);
/**
* Action of the symmetry group on the state space (Equation 4)
* @param X Group element
* @param xi State
* @return New state after group action
*/
template <size_t N>
State<N> operator*(const G<N>& X, const State<N>& xi);
/**
* Action of the symmetry group on the input space (Equation 5)
* @param X Group element
* @param u Input
* @return New input after group action
*/
template <size_t N>
Input velocityAction(const G<N>& X, const Input& u);
/**
* Action of the symmetry group on the output space (Equation 6)
* @param X Group element
* @param y Direction measurement
* @param idx Calibration index
* @return New direction after group action
*/
template <size_t N>
Vector3 outputAction(const G<N>& X, const Unit3& y, int idx);
/**
* Differential of the phi action at E = Id in local coordinates
* @param xi State
* @return Differential matrix
*/
template <size_t N>
Matrix stateActionDiff(const State<N>& xi);
//========================================================================
// Equivariant Filter (EqF)
//========================================================================
/// Equivariant Filter (EqF) implementation
template <size_t N>
class EqF {
private:
int dof; // Degrees of freedom
G<N> X_hat; // Filter state
Matrix Sigma; // Error covariance
State<N> xi_0; // Origin state
Matrix Dphi0; // Differential of phi at origin
Matrix InnovationLift; // Innovation lift matrix
/**
* Return the state matrix A0t (Equation 14a)
* @param u Input
* @return State matrix A0t
*/
Matrix stateMatrixA(const Input& u) const;
/**
* Return the state transition matrix Phi (Equation 17)
* @param u Input
* @param dt Time step
* @return State transition matrix Phi
*/
Matrix stateTransitionMatrix(const Input& u, double dt) const;
/**
* Return the Input matrix Bt
* @return Input matrix Bt
*/
Matrix inputMatrixBt() const;
/**
* Return the measurement matrix C0 (Equation 14b)
* @param d Known direction
* @param idx Calibration index
* @return Measurement matrix C0
*/
Matrix measurementMatrixC(const Unit3& d, int idx) const;
/**
* Return the measurement output matrix Dt
* @param idx Calibration index
* @return Measurement output matrix Dt
*/
Matrix outputMatrixDt(int idx) const;
public:
/**
* Initialize EqF
* @param Sigma Initial covariance
* @param m Number of sensors
*/
EqF(const Matrix& Sigma, int m);
/**
* Return estimated state
* @return Current state estimate
*/
State<N> stateEstimate() const;
/**
* Propagate the filter state
* @param u Angular velocity measurement
* @param dt Time step
*/
void propagation(const Input& u, double dt);
/**
* Update the filter state with a measurement
* @param y Direction measurement
*/
void update(const Measurement& y);
};
//========================================================================
// Helper Functions Implementation
//========================================================================
/**
* Maps system dynamics to the symmetry group
* @param xi State
* @param u Input
* @return Lifted input in Lie Algebra
* Uses Vector zero & Rot3 inverse, matrix functions
*/
template <size_t N>
Vector lift(const State<N>& xi, const Input& u) {
Vector L = Vector::Zero(6 + 3 * N);
// First 3 elements
L.head<3>() = u.w - xi.b;
// Next 3 elements
L.segment<3>(3) = -u.W() * xi.b;
// Remaining elements
for (size_t i = 0; i < N; i++) {
L.segment<3>(6 + 3 * i) = xi.S[i].inverse().matrix() * L.head<3>();
}
return L;
}
/**
* Implements group actions on the states
* @param 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
* @param xi State object
* xi.R -> Attitude (Rot3)
* xi.b -> Gyroscope Bias(Vector 3)
* xi.S -> Vector of calibration matrices(Rot3)
* @return Transformed state
* Uses the Rot3 inverse and Vee functions
*/
template <size_t N>
State<N> operator*(const G<N>& X, const State<N>& xi) {
std::array<Rot3, N> new_S;
for (size_t i = 0; i < N; i++) {
new_S[i] = X.A.inverse() * xi.S[i] * X.B[i];
}
return State<N>(xi.R * X.A, X.A.inverse().matrix() * (xi.b - Rot3::Vee(X.a)),
new_S);
}
/**
* Transforms the angular velocity measurements b/w frames
* @param X A symmetry group element X with the components
* @param u Inputs
* @return Transformed inputs
* Uses Rot3 Inverse, matrix and Vee functions and is critical for maintaining
* the input equivariance
*/
template <size_t N>
Input velocityAction(const G<N>& X, const Input& u) {
return Input{X.A.inverse().matrix() * (u.w - Rot3::Vee(X.a)), u.Sigma};
}
/**
* Transforms the Direction measurements based on the calibration type ( Eqn 6)
* @param X Group element X
* @param y Direction measurement y
* @param idx Calibration index
* @return Transformed direction
* Uses Rot3 inverse, matric and Unit3 unitvector functions
*/
template <size_t N>
Vector3 outputAction(const G<N>& X, const Unit3& y, int idx) {
if (idx == -1) {
return X.A.inverse().matrix() * y.unitVector();
} else {
if (idx >= static_cast<int>(N)) {
throw std::out_of_range("Calibration index out of range");
}
return X.B[idx].inverse().matrix() * y.unitVector();
}
}
/**
* @brief Calculates the Jacobian matrix using central difference approximation
* @param f Vector function f
* @param x The point at which Jacobian is evaluated
* @return Matrix containing numerical partial derivatives of f at x
* Uses Vector's size() and Zero(), Matrix's Zero() and col() methods
*/
Matrix numericalDifferential(std::function<Vector(const Vector&)> f,
const Vector& x) {
double h = 1e-6;
Vector fx = f(x);
int n = fx.size();
int m = x.size();
Matrix Df = Matrix::Zero(n, m);
for (int j = 0; j < m; j++) {
Vector ej = Vector::Zero(m);
ej(j) = 1.0;
Vector fplus = f(x + h * ej);
Vector fminus = f(x - h * ej);
Df.col(j) = (fplus - fminus) / (2 * h);
}
return Df;
}
/**
* Computes the differential of a state action at the identity of the symmetry
* group
* @param xi State object Xi representing the point at which to evaluate the
* differential
* @return A matrix representing the jacobian of the state action
* Uses numericalDifferential, and Rot3 expmap, logmap
*/
template <size_t N>
Matrix stateActionDiff(const State<N>& xi) {
std::function<Vector(const Vector&)> coordsAction = [&xi](const Vector& U) {
G<N> groupElement = G<N>::exp(U);
State<N> transformed = groupElement * xi;
return xi.localCoordinates(transformed);
};
Vector zeros = Vector::Zero(6 + 3 * N);
Matrix differential = numericalDifferential(coordsAction, zeros);
return differential;
}
//========================================================================
// Equivariant Filter (EqF) Implementation
//========================================================================
/**
* Initializes the EqF with state dimension validation and computes lifted
* innovation mapping
* @param Sigma Initial covariance
* @param n Number of calibration states
* @param m Number of sensors
* Uses SelfAdjointSolver, completeOrthoganalDecomposition().pseudoInverse()
*/
template <size_t N>
EqF<N>::EqF(const Matrix& Sigma, int m)
: dof(6 + 3 * N),
X_hat(G<N>::identity(N)),
Sigma(Sigma),
xi_0(State<N>::identity()) {
if (Sigma.rows() != dof || Sigma.cols() != dof) {
throw std::invalid_argument(
"Initial covariance dimensions must match the degrees of freedom");
}
// Check positive semi-definite
Eigen::SelfAdjointEigenSolver<Matrix> eigensolver(Sigma);
if (eigensolver.eigenvalues().minCoeff() < -1e-10) {
throw std::invalid_argument(
"Covariance matrix must be semi-positive definite");
}
if (N < 0) {
throw std::invalid_argument(
"Number of calibration states must be non-negative");
}
if (m <= 1) {
throw std::invalid_argument(
"Number of direction sensors must be at least 2");
}
// Compute differential of phi
Dphi0 = stateActionDiff(xi_0);
InnovationLift = Dphi0.completeOrthogonalDecomposition().pseudoInverse();
}
/**
* Computes the internal group state to a physical state estimate
* @return Current state estimate
*/
template <size_t N>
State<N> EqF<N>::stateEstimate() const {
return X_hat * xi_0;
}
/**
* 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
* @param u Angular velocity measurements
* @param dt time steps
* Updated internal state and covariance
*/
template <size_t N>
void EqF<N>::propagation(const Input& u, double dt) {
State<N> state_est = stateEstimate();
Vector L = lift(state_est, u);
Matrix Phi_DT = stateTransitionMatrix(u, dt);
Matrix Bt = inputMatrixBt();
Matrix tempSigma = blockDiag(u.Sigma, repBlock(1e-9 * I_3x3, N));
Matrix M_DT = (Bt * tempSigma * Bt.transpose()) * dt;
X_hat = X_hat * G<N>::exp(L * dt);
Sigma = Phi_DT * Sigma * Phi_DT.transpose() + M_DT;
}
/**
* Implements the correction step of the filter using discrete measurements
* Computes the measurement residual, Kalman gain and the updates both the state
* and covariance
*
* @param y Measurements
*/
template <size_t N>
void EqF<N>::update(const Measurement& y) {
if (y.cal_idx > static_cast<int>(N)) {
throw std::invalid_argument("Calibration index out of range");
}
// Get vector representations for checking
Vector3 y_vec = y.y.unitVector();
Vector3 d_vec = y.d.unitVector();
// Skip update if any NaN values are present
if (std::isnan(y_vec[0]) || std::isnan(y_vec[1]) || std::isnan(y_vec[2]) ||
std::isnan(d_vec[0]) || std::isnan(d_vec[1]) || std::isnan(d_vec[2])) {
return; // Skip this measurement
}
Matrix Ct = measurementMatrixC(y.d, y.cal_idx);
Vector3 action_result = outputAction(X_hat.inv(), y.y, y.cal_idx);
Vector3 delta_vec = Rot3::Hat(y.d.unitVector()) * action_result;
Matrix Dt = outputMatrixDt(y.cal_idx);
Matrix S = Ct * Sigma * Ct.transpose() + Dt * y.Sigma * Dt.transpose();
Matrix K = Sigma * Ct.transpose() * S.inverse();
Vector Delta = InnovationLift * K * delta_vec;
X_hat = G<N>::exp(Delta) * X_hat;
Sigma = (Matrix::Identity(dof, dof) - K * Ct) * Sigma;
}
/**
* Computes linearized continuous time state matrix
* @param u Angular velocity
* @return Linearized state matrix
* Uses Matrix zero and Identity functions
*/
template <size_t N>
Matrix EqF<N>::stateMatrixA(const Input& u) const {
Matrix3 W0 = velocityAction(X_hat.inv(), u).W();
Matrix A1 = Matrix::Zero(6, 6);
A1.block<3, 3>(0, 3) = -I_3x3;
A1.block<3, 3>(3, 3) = W0;
Matrix A2 = repBlock(W0, N);
return blockDiag(A1, A2);
}
/**
* Computes the discrete time state transition matrix
* @param u Angular velocity
* @param dt time step
* @return State transition matrix in discrete time
*/
template <size_t N>
Matrix EqF<N>::stateTransitionMatrix(const Input& u, double dt) const {
Matrix3 W0 = velocityAction(X_hat.inv(), u).W();
Matrix Phi1 = Matrix::Zero(6, 6);
Matrix3 Phi12 = -dt * (I_3x3 + (dt / 2) * W0 + ((dt * dt) / 6) * W0 * W0);
Matrix3 Phi22 = I_3x3 + dt * W0 + ((dt * dt) / 2) * W0 * W0;
Phi1.block<3, 3>(0, 0) = I_3x3;
Phi1.block<3, 3>(0, 3) = Phi12;
Phi1.block<3, 3>(3, 3) = Phi22;
Matrix Phi2 = repBlock(Phi22, N);
return blockDiag(Phi1, Phi2);
}
/**
* Computes the input uncertainty propagation matrix
* @return
* Uses the blockdiag matrix
*/
template <size_t N>
Matrix EqF<N>::inputMatrixBt() const {
Matrix B1 = blockDiag(X_hat.A.matrix(), X_hat.A.matrix());
Matrix B2(3 * N, 3 * N);
for (size_t i = 0; i < N; ++i) {
B2.block<3, 3>(3 * i, 3 * i) = X_hat.B[i].matrix();
}
return blockDiag(B1, B2);
}
/**
* Computes the linearized measurement matrix. The structure depends on whether
* the sensor has a calibration state
* @param d reference direction
* @param idx Calibration index
* @return Measurement matrix
* Uses the matrix zero, Rot3 hat and the Unitvector functions
*/
template <size_t N>
Matrix EqF<N>::measurementMatrixC(const Unit3& d, int idx) const {
Matrix Cc = Matrix::Zero(3, 3 * N);
// If the measurement is related to a sensor that has a calibration state
if (idx >= 0) {
// Set the correct 3x3 block in Cc
Cc.block<3, 3>(0, 3 * idx) = Rot3::Hat(d.unitVector());
}
Matrix3 wedge_d = Rot3::Hat(d.unitVector());
// Create the combined matrix
Matrix temp(3, 6 + 3 * N);
temp.block<3, 3>(0, 0) = wedge_d;
temp.block<3, 3>(0, 3) = Matrix3::Zero();
temp.block(0, 6, 3, 3 * N) = Cc;
return wedge_d * temp;
}
/**
* Computes the measurement uncertainty propagation matrix
* @param idx Calibration index
* @return Returns B[idx] for calibrated sensors, A for uncalibrated
*/
template <size_t N>
Matrix EqF<N>::outputMatrixDt(int idx) const {
// If the measurement is related to a sensor that has a calibration state
if (idx >= 0) {
if (idx >= static_cast<int>(N)) {
throw std::out_of_range("Calibration index out of range");
}
return X_hat.B[idx].matrix();
} else {
return X_hat.A.matrix();
}
}
} // namespace abc_eqf_lib
template <size_t N>
struct traits<abc_eqf_lib::EqF<N>>
: internal::LieGroupTraits<abc_eqf_lib::EqF<N>> {};
} // namespace gtsam
#endif // ABC_EQF_H

444
examples/ABC_EQF_Demo.cpp Normal file
View File

@ -0,0 +1,444 @@
/**
* @file ABC_EQF_Demo.cpp
* @brief Demonstration of the full Attitude-Bias-Calibration Equivariant Filter
*
* This demo shows the Equivariant Filter (EqF) for attitude estimation
* with both gyroscope bias and sensor extrinsic calibration, based on the
* paper: "Overcoming Bias: Equivariant Filter Design for Biased Attitude
* Estimation with Online Calibration" by Fornasier et al. Authors: Darshan
* Rajasekaran & Jennifer Oum
*/
#include "ABC_EQF.h"
// Use namespace for convenience
using namespace gtsam;
constexpr size_t N = 1; // Number of calibration states
using M = abc_eqf_lib::State<N>;
using Group = abc_eqf_lib::G<N>;
using EqFilter = abc_eqf_lib::EqF<N>;
using gtsam::abc_eqf_lib::EqF;
using gtsam::abc_eqf_lib::Input;
using gtsam::abc_eqf_lib::Measurement;
/// Data structure for ground-truth, input and output data
struct Data {
M xi; /// Ground-truth state
Input u; /// Input measurements
std::vector<Measurement> y; /// Output measurements
int n_meas; /// Number of measurements
double t; /// Time
double dt; /// Time step
};
//========================================================================
// Data Processing Functions
//========================================================================
/**
* Load data from CSV file into a vector of Data objects for the EqF
*
* CSV format:
* - t: Time
* - q_w, q_x, q_y, q_z: True attitude quaternion
* - b_x, b_y, b_z: True bias
* - cq_w_0, cq_x_0, cq_y_0, cq_z_0: True calibration quaternion
* - w_x, w_y, w_z: Angular velocity measurements
* - std_w_x, std_w_y, std_w_z: Angular velocity measurement standard deviations
* - std_b_x, std_b_y, std_b_z: Bias process noise standard deviations
* - y_x_0, y_y_0, y_z_0, y_x_1, y_y_1, y_z_1: Direction measurements
* - std_y_x_0, std_y_y_0, std_y_z_0, std_y_x_1, std_y_y_1, std_y_z_1: Direction
* measurement standard deviations
* - d_x_0, d_y_0, d_z_0, d_x_1, d_y_1, d_z_1: Reference directions
*
*/
std::vector<Data> loadDataFromCSV(const std::string& filename, int startRow = 0,
int maxRows = -1, int downsample = 1);
/// Process data with EqF and print summary results
void processDataWithEqF(EqFilter& filter, const std::vector<Data>& data_list,
int printInterval = 10);
//========================================================================
// Data Processing Functions Implementation
//========================================================================
/*
* Loads the test data from the csv file
* startRow First row to load based on csv, 0 by default
* maxRows maximum rows to load, defaults to all rows
* downsample Downsample factor, default 1
* A list of data objects
*/
std::vector<Data> loadDataFromCSV(const std::string& filename, int startRow,
int maxRows, int downsample) {
std::vector<Data> data_list;
std::ifstream file(filename);
if (!file.is_open()) {
throw std::runtime_error("Failed to open file: " + filename);
}
std::cout << "Loading data from " << filename << "..." << std::flush;
std::string line;
int lineNumber = 0;
int rowCount = 0;
int errorCount = 0;
double prevTime = 0.0;
// Skip header
std::getline(file, line);
lineNumber++;
// Skip to startRow
while (lineNumber < startRow && std::getline(file, line)) {
lineNumber++;
}
// Read data
while (std::getline(file, line) && (maxRows == -1 || rowCount < maxRows)) {
lineNumber++;
// Apply downsampling
if ((lineNumber - startRow - 1) % downsample != 0) {
continue;
}
std::istringstream ss(line);
std::string token;
std::vector<double> values;
// Parse line into values
while (std::getline(ss, token, ',')) {
try {
values.push_back(std::stod(token));
} catch (const std::exception& e) {
errorCount++;
values.push_back(0.0); // Use default value
}
}
// Check if we have enough values
if (values.size() < 39) {
errorCount++;
continue;
}
// Extract values
double t = values[0];
double dt = (rowCount == 0) ? 0.0 : t - prevTime;
prevTime = t;
// Create ground truth state
Quaternion quat(values[1], values[2], values[3], values[4]); // w, x, y, z
Rot3 R = Rot3(quat);
Vector3 b(values[5], values[6], values[7]);
Quaternion calQuat(values[8], values[9], values[10],
values[11]); // w, x, y, z
std::array<Rot3, N> S = {Rot3(calQuat)};
M xi(R, b, S);
// Create input
Vector3 w(values[12], values[13], values[14]);
// Create input covariance matrix (6x6)
// First 3x3 block for angular velocity, second 3x3 block for bias process
// noise
Matrix inputCov = Matrix::Zero(6, 6);
inputCov(0, 0) = values[15] * values[15]; // std_w_x^2
inputCov(1, 1) = values[16] * values[16]; // std_w_y^2
inputCov(2, 2) = values[17] * values[17]; // std_w_z^2
inputCov(3, 3) = values[18] * values[18]; // std_b_x^2
inputCov(4, 4) = values[19] * values[19]; // std_b_y^2
inputCov(5, 5) = values[20] * values[20]; // std_b_z^2
Input u{w, inputCov};
// Create measurements
std::vector<Measurement> measurements;
// First measurement (calibrated sensor, cal_idx = 0)
Vector3 y0(values[21], values[22], values[23]);
Vector3 d0(values[33], values[34], values[35]);
// Normalize vectors if needed
if (abs(y0.norm() - 1.0) > 1e-5) y0.normalize();
if (abs(d0.norm() - 1.0) > 1e-5) d0.normalize();
// Measurement covariance
Matrix3 covY0 = Matrix3::Zero();
covY0(0, 0) = values[27] * values[27]; // std_y_x_0^2
covY0(1, 1) = values[28] * values[28]; // std_y_y_0^2
covY0(2, 2) = values[29] * values[29]; // std_y_z_0^2
// Create measurement
measurements.push_back(Measurement{Unit3(y0), Unit3(d0), covY0, 0});
// Second measurement (calibrated sensor, cal_idx = -1)
Vector3 y1(values[24], values[25], values[26]);
Vector3 d1(values[36], values[37], values[38]);
// Normalize vectors if needed
if (abs(y1.norm() - 1.0) > 1e-5) y1.normalize();
if (abs(d1.norm() - 1.0) > 1e-5) d1.normalize();
// Measurement covariance
Matrix3 covY1 = Matrix3::Zero();
covY1(0, 0) = values[30] * values[30]; // std_y_x_1^2
covY1(1, 1) = values[31] * values[31]; // std_y_y_1^2
covY1(2, 2) = values[32] * values[32]; // std_y_z_1^2
// Create measurement
measurements.push_back(Measurement{Unit3(y1), Unit3(d1), covY1, -1});
// Create Data object and add to list
data_list.push_back(Data{xi, u, measurements, 2, t, dt});
rowCount++;
// Show loading progress every 1000 rows
if (rowCount % 1000 == 0) {
std::cout << "." << std::flush;
}
}
std::cout << " Done!" << std::endl;
std::cout << "Loaded " << data_list.size() << " data points";
if (errorCount > 0) {
std::cout << " (" << errorCount << " errors encountered)";
}
std::cout << std::endl;
return data_list;
}
/// Takes in the data and runs an EqF on it and reports the results
void processDataWithEqF(EqFilter& filter, const std::vector<Data>& data_list,
int printInterval) {
if (data_list.empty()) {
std::cerr << "No data to process" << std::endl;
return;
}
std::cout << "Processing " << data_list.size() << " data points with EqF..."
<< std::endl;
// Track performance metrics
std::vector<double> att_errors;
std::vector<double> bias_errors;
std::vector<double> cal_errors;
// Track time for performance measurement
auto start = std::chrono::high_resolution_clock::now();
int totalMeasurements = 0;
int validMeasurements = 0;
// Define constant for converting radians to degrees
const double RAD_TO_DEG = 180.0 / M_PI;
// Print a progress indicator
int progressStep = data_list.size() / 10; // 10 progress updates
if (progressStep < 1) progressStep = 1;
std::cout << "Progress: ";
for (size_t i = 0; i < data_list.size(); i++) {
const Data& data = data_list[i];
// Propagate filter with current input and time step
filter.propagation(data.u, data.dt);
// Process all measurements
for (const auto& y : data.y) {
totalMeasurements++;
// Skip invalid measurements
Vector3 y_vec = y.y.unitVector();
Vector3 d_vec = y.d.unitVector();
if (std::isnan(y_vec[0]) || std::isnan(y_vec[1]) ||
std::isnan(y_vec[2]) || std::isnan(d_vec[0]) ||
std::isnan(d_vec[1]) || std::isnan(d_vec[2])) {
continue;
}
try {
filter.update(y);
validMeasurements++;
} catch (const std::exception& e) {
std::cerr << "Error updating at t=" << data.t << ": " << e.what()
<< std::endl;
}
}
// Get current state estimate
M estimate = filter.stateEstimate();
// Calculate errors
Vector3 att_error = Rot3::Logmap(data.xi.R.between(estimate.R));
Vector3 bias_error = estimate.b - data.xi.b;
Vector3 cal_error = Vector3::Zero();
if (!data.xi.S.empty() && !estimate.S.empty()) {
cal_error = Rot3::Logmap(data.xi.S[0].between(estimate.S[0]));
}
// Store errors
att_errors.push_back(att_error.norm());
bias_errors.push_back(bias_error.norm());
cal_errors.push_back(cal_error.norm());
// Show progress dots
if (i % progressStep == 0) {
std::cout << "." << std::flush;
}
}
std::cout << " Done!" << std::endl;
auto end = std::chrono::high_resolution_clock::now();
std::chrono::duration<double> elapsed = end - start;
// Calculate average errors
double avg_att_error = 0.0;
double avg_bias_error = 0.0;
double avg_cal_error = 0.0;
if (!att_errors.empty()) {
avg_att_error = std::accumulate(att_errors.begin(), att_errors.end(), 0.0) /
att_errors.size();
avg_bias_error =
std::accumulate(bias_errors.begin(), bias_errors.end(), 0.0) /
bias_errors.size();
avg_cal_error = std::accumulate(cal_errors.begin(), cal_errors.end(), 0.0) /
cal_errors.size();
}
// Calculate final errors from last data point
const Data& final_data = data_list.back();
M final_estimate = filter.stateEstimate();
Vector3 final_att_error =
Rot3::Logmap(final_data.xi.R.between(final_estimate.R));
Vector3 final_bias_error = final_estimate.b - final_data.xi.b;
Vector3 final_cal_error = Vector3::Zero();
if (!final_data.xi.S.empty() && !final_estimate.S.empty()) {
final_cal_error =
Rot3::Logmap(final_data.xi.S[0].between(final_estimate.S[0]));
}
// Print summary statistics
std::cout << "\n=== Filter Performance Summary ===" << std::endl;
std::cout << "Processing time: " << elapsed.count() << " seconds"
<< std::endl;
std::cout << "Processed measurements: " << totalMeasurements
<< " (valid: " << validMeasurements << ")" << std::endl;
// Average errors
std::cout << "\n-- Average Errors --" << std::endl;
std::cout << "Attitude: " << (avg_att_error * RAD_TO_DEG) << "°" << std::endl;
std::cout << "Bias: " << avg_bias_error << std::endl;
std::cout << "Calibration: " << (avg_cal_error * RAD_TO_DEG) << "°"
<< std::endl;
// Final errors
std::cout << "\n-- Final Errors --" << std::endl;
std::cout << "Attitude: " << (final_att_error.norm() * RAD_TO_DEG) << "°"
<< std::endl;
std::cout << "Bias: " << final_bias_error.norm() << std::endl;
std::cout << "Calibration: " << (final_cal_error.norm() * RAD_TO_DEG) << "°"
<< std::endl;
// Print a brief comparison of final estimate vs ground truth
std::cout << "\n-- Final State vs Ground Truth --" << std::endl;
std::cout << "Attitude (RPY) - Estimate: "
<< (final_estimate.R.rpy() * RAD_TO_DEG).transpose()
<< "° | Truth: " << (final_data.xi.R.rpy() * RAD_TO_DEG).transpose()
<< "°" << std::endl;
std::cout << "Bias - Estimate: " << final_estimate.b.transpose()
<< " | Truth: " << final_data.xi.b.transpose() << std::endl;
if (!final_estimate.S.empty() && !final_data.xi.S.empty()) {
std::cout << "Calibration (RPY) - Estimate: "
<< (final_estimate.S[0].rpy() * RAD_TO_DEG).transpose()
<< "° | Truth: "
<< (final_data.xi.S[0].rpy() * RAD_TO_DEG).transpose() << "°"
<< std::endl;
}
}
int main(int argc, char* argv[]) {
std::cout << "ABC-EqF: Attitude-Bias-Calibration Equivariant Filter Demo"
<< std::endl;
std::cout << "=============================================================="
<< std::endl;
try {
// Parse command line options
std::string csvFilePath;
int maxRows = -1; // Process all rows by default
int downsample = 1; // No downsampling by default
if (argc > 1) {
csvFilePath = argv[1];
} else {
// Try to find the EQFdata file in the GTSAM examples directory
try {
csvFilePath = findExampleDataFile("EqFdata.csv");
} catch (const std::exception& e) {
std::cerr << "Error: Could not find EqFdata.csv" << std::endl;
std::cerr << "Usage: " << argv[0]
<< " [csv_file_path] [max_rows] [downsample]" << std::endl;
return 1;
}
}
// Optional command line parameters
if (argc > 2) {
maxRows = std::stoi(argv[2]);
}
if (argc > 3) {
downsample = std::stoi(argv[3]);
}
// Load data from CSV file
std::vector<Data> data =
loadDataFromCSV(csvFilePath, 0, maxRows, downsample);
if (data.empty()) {
std::cerr << "No data available to process. Exiting." << std::endl;
return 1;
}
// Initialize the EqF filter with one calibration state
int n_sensors = 2;
// Initial covariance - larger values allow faster convergence
Matrix initialSigma = Matrix::Identity(6 + 3 * N, 6 + 3 * N);
initialSigma.diagonal().head<3>() =
Vector3::Constant(0.1); // Attitude uncertainty
initialSigma.diagonal().segment<3>(3) =
Vector3::Constant(0.01); // Bias uncertainty
initialSigma.diagonal().tail<3>() =
Vector3::Constant(0.1); // Calibration uncertainty
// Create filter
EqFilter filter(initialSigma, n_sensors);
// Process data
processDataWithEqF(filter, data);
} catch (const std::exception& e) {
std::cerr << "Error: " << e.what() << std::endl;
return 1;
}
std::cout << "\nEqF demonstration completed successfully." << std::endl;
return 0;
}

2
gtsam/3rdparty/metis/CMakeLists.txt vendored
View File

@ -65,4 +65,4 @@ if (TARGET metis AND GKlib_COPTIONS)
separate_arguments(GKlib_COPTIONS) separate_arguments(GKlib_COPTIONS)
# Declare those flags as to-be-imported in "client libraries", i.e. "gtsam" # Declare those flags as to-be-imported in "client libraries", i.e. "gtsam"
target_compile_options(metis INTERFACE ${GKlib_COPTIONS}) target_compile_options(metis INTERFACE ${GKlib_COPTIONS})
endif() endif()