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

C++ implementation of the EqF to estimate bias.

release/4.3a0
darshan-17 2025-04-15 21:06:25 -07:00 committed by jenniferoum
parent 39a7a5b627
commit cd1782e5d4
23 changed files with 1529 additions and 1794 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1009
examples/ABC_EQF.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

427
examples/ABC_EQF.h Normal file
View File

@ -0,0 +1,427 @@
/**
* @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/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/navigation/ImuBias.h>
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <cmath>
#include <functional>
#include <chrono>
#include <numeric> // For std::accumulate
// All implementations are wrapped in this namespace to avoid conflicts
namespace abc_eqf_lib {
using namespace std;
using namespace gtsam;
// Global configuration
// Define coordinate type: "EXPONENTIAL" or "NORMAL"
extern const std::string COORDINATE;
//========================================================================
// 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);
/**
* Calculate numerical differential
*/
Matrix numericalDifferential(std::function<Vector(const Vector&)> f, const Vector& x);
//========================================================================
// Core Data Types
//========================================================================
/**
* Direction class as a S2 element
*/
class Direction {
public:
Unit3 d; // Direction (unit vector on S2)
/**
* Initialize direction
* @param d_vec Direction vector (must be unit norm)
*/
Direction(const Vector3& d_vec);
// Accessor methods for vector components
double x() const;
double y() const;
double z() const;
// Check if the direction contains NaN values
bool hasNaN() const;
};
/**
* Input class for the Biased Attitude System
*/
class Input {
public:
Vector3 w; // Angular velocity
Matrix Sigma; // Noise covariance
/**
* Initialize Input
* @param w Angular velocity (3-vector)
* @param Sigma Noise covariance (6x6 matrix)
*/
Input(const Vector3& w, const Matrix& Sigma);
/**
* Return the Input as a skew-symmetric matrix
* @return w as a skew-symmetric matrix
*/
Matrix3 W() const;
/**
* Return a random angular velocity
* @return A random angular velocity as Input element
*/
static Input random();
};
/**
* Measurement class
* cal_idx is an index corresponding to the calibration related to the measurement
* cal_idx = -1 indicates the measurement is from a calibrated sensor
*/
class Measurement {
public:
Direction y; // Measurement direction in sensor frame
Direction d; // Known direction in global frame
Matrix3 Sigma; // Covariance matrix of the measurement
int cal_idx = -1; // Calibration index (-1 for calibrated sensor)
/**
* Initialize measurement
* @param y_vec Direction measurement in sensor frame
* @param d_vec Known direction in global frame
* @param Sigma Measurement noise covariance
* @param i Calibration index (-1 for calibrated sensor)
*/
Measurement(const Vector3& y_vec, const Vector3& d_vec,
const Matrix3& Sigma, int i = -1);
};
/**
* State class representing the state of the Biased Attitude System
*/
class State {
public:
Rot3 R; // Attitude rotation matrix R
Vector3 b; // Gyroscope bias b
std::vector<Rot3> S; // Sensor calibrations S
State(const Rot3& R = Rot3::Identity(),
const Vector3& b = Vector3::Zero(),
const std::vector<Rot3>& S = std::vector<Rot3>());
static State identity(int n);
};
/**
* Data structure for ground-truth, input and output data
*/
struct Data {
State xi; // Ground-truth state
int n_cal; // Number of calibration states
Input u; // Input measurements
std::vector<Measurement> y; // Output measurements
int n_meas; // Number of measurements
double t; // Time
double dt; // Time step
/**
* Initialize Data
* @param xi Ground-truth state
* @param n_cal Number of calibration states
* @param u Input measurements
* @param y Output measurements
* @param n_meas Number of measurements
* @param t Time
* @param dt Time step
*/
Data(const State& xi, int n_cal, const Input& u,
const std::vector<Measurement>& y, int n_meas,
double t, double dt);
};
//========================================================================
// 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
*/
class G {
public:
Rot3 A; // First SO(3) element
Matrix3 a; // so(3) element (skew-symmetric matrix)
std::vector<Rot3> B; // List of SO(3) elements for calibration
/**
* Initialize the symmetry group G
* @param A SO3 element
* @param a so(3) element (skew symmetric matrix)
* @param B list of SO3 elements
*/
G(const Rot3& A = Rot3::Identity(),
const Matrix3& a = Matrix3::Zero(),
const std::vector<Rot3>& B = std::vector<Rot3>());
/**
* Define the group operation (multiplication)
* @param other Another group element
* @return The product of this and other
*/
G operator*(const G& other) const;
/**
* Return the inverse element of the symmetry group
* @return The inverse of this group element
*/
G inv() const;
/**
* Return the identity of the symmetry group
* @param n Number of calibration elements
* @return The identity element with n calibration components
*/
static G identity(int n);
/**
* Return a group element X given by X = exp(x)
* @param x Vector representation of Lie algebra element
* @return Group element given by the exponential of x
*/
static G exp(const Vector& x);
};
//========================================================================
// Helper Functions for EqF
//========================================================================
/**
* Compute the lift of the system (Theorem 3.8, Equation 7)
* @param xi State
* @param u Input
* @return Lift vector
*/
Vector lift(const State& 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
*/
State stateAction(const G& X, const State& 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
*/
Input velocityAction(const G& 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
*/
Vector3 outputAction(const G& X, const Direction& y, int idx);
/**
* Local coordinates assuming xi_0 = identity (Equation 9)
* @param e State representing equivariant error
* @return Local coordinates
*/
Vector local_coords(const State& e);
/**
* Local coordinates inverse assuming xi_0 = identity
* @param eps Local coordinates
* @return Corresponding state
*/
State local_coords_inv(const Vector& eps);
/**
* Differential of the phi action at E = Id in local coordinates
* @param xi State
* @return Differential matrix
*/
Matrix stateActionDiff(const State& xi);
//========================================================================
// Equivariant Filter (EqF)
//========================================================================
/**
* Equivariant Filter (EqF) implementation
*/
class EqF {
private:
int __dof; // Degrees of freedom
int __n_cal; // Number of calibration states
int __n_sensor; // Number of sensors
G __X_hat; // Filter state
Matrix __Sigma; // Error covariance
State __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 Direction& 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 n Number of calibration states
* @param m Number of sensors
*/
EqF(const Matrix& Sigma, int n, int m);
/**
* Return estimated state
* @return Current state estimate
*/
State 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);
};
//========================================================================
// 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
*
* @param filename Path to the CSV file
* @param startRow First row to load (default: 0)
* @param maxRows Maximum number of rows to load (default: all)
* @param downsample Downsample factor (default: 1, which means no downsampling)
* @return Vector of Data objects
*/
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
* @param filter Initialized EqF filter
* @param data_list Vector of Data objects to process
* @param printInterval Progress indicator interval (used internally)
*/
void processDataWithEqF(EqF& filter, const std::vector<Data>& data_list, int printInterval = 10);
} // namespace abc_eqf_lib
#endif // ABC_EQF_H

14
examples/ABC_EQF/CMakeLists.txt
View File

@ -1,14 +0,0 @@
add_executable(ABC_EqF
main.cpp
EqF.cpp
State.cpp
Input.cpp
G.cpp
Direction.cpp
Measurements.cpp
utilities.cpp
runEQF_withcsv.h
)
target_link_libraries(ABC_EqF gtsam)

41
examples/ABC_EQF/Data.h
View File

@ -1,41 +0,0 @@
//
// Created by darshan on 3/11/25.
//
#ifndef DATA_H
#define DATA_H
//#pragma once
#include "State.h"
#include "Input.h"
#include "Measurements.h"
#include <vector>
/**
* Data structure for ground-truth, input and output data
*/
struct Data {
State xi; // Ground-truth state
int n_cal; // Number of calibration states
Input u; // Input measurements
std::vector<Measurement> y; // Output measurements
int n_meas; // Number of measurements
double t; // Time
double dt; // Time step
/**
* Initialize Data
* @param xi Ground-truth state
* @param n_cal Number of calibration states
* @param u Input measurements
* @param y Output measurements
* @param n_meas Number of measurements
* @param t Time
* @param dt Time step
*/
Data(const State& xi, int n_cal, const Input& u,
const std::vector<Measurement>& y, int n_meas,
double t, double dt)
: xi(xi), n_cal(n_cal), u(u), y(y), n_meas(n_meas), t(t), dt(dt) {}
};
#endif //DATA_H

13
examples/ABC_EQF/Direction.cpp
View File

@ -1,13 +0,0 @@
//
// Created by darshan on 3/11/25.
//
#include "Direction.h"
#include "utilities.h"
#include <stdexcept>
Direction::Direction(const Vector3& d_vec) : d(d_vec) {
if (!checkNorm(d_vec)) {
throw std::invalid_argument("Direction must be a unit vector");
}
}

35
examples/ABC_EQF/Direction.h
View File

@ -1,35 +0,0 @@
//
// Created by darshan on 3/11/25.
//
#ifndef DIRECTION_H
#define DIRECTION_H
//#pragma once
#include <gtsam/geometry/Unit3.h>
#include <gtsam/base/Vector.h>
using namespace gtsam;
/**
* Direction class as a S2 element
*/
class Direction {
public:
Unit3 d; // Direction (unit vector on S2)
/**
* Initialize direction
* @param d_vec Direction vector (must be unit norm)
*/
Direction(const Vector3& d_vec);
// Accessor methods for vector components
double x() const { return d.unitVector()[0]; }
double y() const { return d.unitVector()[1]; }
double z() const { return d.unitVector()[2]; }
// Check if the direction contains NaN values
bool hasNaN() const {
Vector3 vec = d.unitVector();
return std::isnan(vec[0]) || std::isnan(vec[1]) || std::isnan(vec[2]);
}
};
#endif //DIRECTION_H

285
examples/ABC_EQF/EqF.cpp
View File

@ -1,285 +0,0 @@
//
// Created by darshan on 3/11/25.
//
#include "EqF.h"
#include "utilities.h"
#include <Eigen/Dense>
#include <stdexcept>
#include <functional>
// Implementation of helper functions
Vector lift(const State& xi, const Input& u) {
int n = xi.S.size();
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 (int i = 0; i < n; i++) {
L.segment<3>(6 + 3*i) = xi.S[i].inverse().matrix() * L.head<3>();
}
return L;
}
State stateAction(const G& X, const State& xi) {
if (xi.S.size() != X.B.size()) {
throw std::invalid_argument("Number of calibration states and B elements must match");
}
std::vector<Rot3> new_S;
for (size_t i = 0; i < X.B.size(); i++) {
new_S.push_back(X.A.inverse() * xi.S[i] * X.B[i]);
}
return State(xi.R * X.A,
X.A.inverse().matrix() * (xi.b - Rot3::Vee(X.a)),
new_S);
}
Input velocityAction(const G& X, const Input& u) {
return Input(X.A.inverse().matrix() * (u.w - Rot3::Vee(X.a)), u.Sigma);
}
Vector3 outputAction(const G& X, const Direction& y, int idx) {
if (idx == -1) {
return X.A.inverse().matrix() * y.d.unitVector();
} else {
if (idx >= static_cast<int>(X.B.size())) {
throw std::out_of_range("Calibration index out of range");
}
return X.B[idx].inverse().matrix() * y.d.unitVector();
}
}
Vector local_coords(const State& e) {
if (COORDINATE == "EXPONENTIAL") {
Vector eps(6 + 3 * e.S.size());
// First 3 elements
eps.head<3>() = Rot3::Logmap(e.R);
// Next 3 elements
eps.segment<3>(3) = e.b;
// Remaining elements
for (size_t i = 0; i < e.S.size(); i++) {
eps.segment<3>(6 + 3*i) = Rot3::Logmap(e.S[i]);
}
return eps;
} else if (COORDINATE == "NORMAL") {
throw std::runtime_error("Normal coordinate representation is not implemented yet");
} else {
throw std::invalid_argument("Invalid coordinate representation");
}
}
State local_coords_inv(const Vector& eps) {
G X = G::exp(eps);
if (COORDINATE == "EXPONENTIAL") {
std::vector<Rot3> S = X.B;
return State(X.A, eps.segment<3>(3), S);
} else if (COORDINATE == "NORMAL") {
throw std::runtime_error("Normal coordinate representation is not implemented yet");
} else {
throw std::invalid_argument("Invalid coordinate representation");
}
}
Matrix stateActionDiff(const State& xi) {
std::function<Vector(const Vector&)> coordsAction =
[&xi](const Vector& U) {
return local_coords(stateAction(G::exp(U), xi));
};
Vector zeros = Vector::Zero(6 + 3 * xi.S.size());
Matrix differential = numericalDifferential(coordsAction, zeros);
return differential;
}
// EqF class implementation
EqF::EqF(const Matrix& Sigma, int n, int m)
: __dof(6 + 3 * n), __n_cal(n), __n_sensor(m), __X_hat(G::identity(n)),
__Sigma(Sigma), __xi_0(State::identity(n)) {
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();
}
State EqF::stateEstimate() const {
return stateAction(__X_hat, __xi_0);
}
void EqF::propagation(const Input& u, double dt) {
State 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 * Matrix3::Identity(), __n_cal));
Matrix M_DT = (Bt * tempSigma * Bt.transpose()) * dt;
__X_hat = __X_hat * G::exp(L * dt);
__Sigma = Phi_DT * __Sigma * Phi_DT.transpose() + M_DT;
}
bool hasNaN(const Vector3& vec) {
return std::isnan(vec[0]) || std::isnan(vec[1]) || std::isnan(vec[2]);
}
void EqF::update(const Measurement& y) {
if (y.cal_idx > __n_cal) {
throw std::invalid_argument("Calibration index out of range");
}
// Get vector representations for checking
Vector3 y_vec = y.y.d.unitVector();
Vector3 d_vec = y.d.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
}
static int update_count = 0;
if (update_count < 5) {
std::cout << "Update " << update_count << ":\n";
std::cout << "y_vec: " << y_vec.transpose() << "\n";
std::cout << "d_vec: " << d_vec.transpose() << "\n";
update_count++;
}
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.d.unitVector()) * action_result; // Ensure this is the right operation
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::exp(Delta) * __X_hat;
__Sigma = (Matrix::Identity(__dof, __dof) - K * Ct) * __Sigma;
}
Matrix EqF::__stateMatrixA(const Input& u) const {
Matrix3 W0 = velocityAction(__X_hat.inv(), u).W();
Matrix A1 = Matrix::Zero(6, 6);
if (COORDINATE == "EXPONENTIAL") {
A1.block<3, 3>(0, 3) = -Matrix3::Identity();
A1.block<3, 3>(3, 3) = W0;
Matrix A2 = repBlock(W0, __n_cal);
return blockDiag(A1, A2);
} else if (COORDINATE == "NORMAL") {
throw std::runtime_error("Normal coordinate representation is not implemented yet");
} else {
throw std::invalid_argument("Invalid coordinate representation");
}
}
Matrix EqF::__stateTransitionMatrix(const Input& u, double dt) const {
Matrix3 W0 = velocityAction(__X_hat.inv(), u).W();
Matrix Phi1 = Matrix::Zero(6, 6);
Matrix3 Phi12 = -dt * (Matrix3::Identity() + (dt / 2) * W0 + ((dt*dt) / 6) * W0 * W0);
Matrix3 Phi22 = Matrix3::Identity() + dt * W0 + ((dt*dt) / 2) * W0 * W0;
if (COORDINATE == "EXPONENTIAL") {
Phi1.block<3, 3>(0, 0) = Matrix3::Identity();
Phi1.block<3, 3>(0, 3) = Phi12;
Phi1.block<3, 3>(3, 3) = Phi22;
Matrix Phi2 = repBlock(Phi22, __n_cal);
return blockDiag(Phi1, Phi2);
} else if (COORDINATE == "NORMAL") {
throw std::runtime_error("Normal coordinate representation is not implemented yet");
} else {
throw std::invalid_argument("Invalid coordinate representation");
}
}
Matrix EqF::__inputMatrixBt() const {
if (COORDINATE == "EXPONENTIAL") {
Matrix B1 = blockDiag(__X_hat.A.matrix(), __X_hat.A.matrix());
Matrix B2;
for (const auto& B : __X_hat.B) {
if (B2.size() == 0) {
B2 = B.matrix();
} else {
B2 = blockDiag(B2, B.matrix());
}
}
return blockDiag(B1, B2);
} else if (COORDINATE == "NORMAL") {
throw std::runtime_error("Normal coordinate representation is not implemented yet");
} else {
throw std::invalid_argument("Invalid coordinate representation");
}
}
Matrix EqF::__measurementMatrixC(const Direction& d, int idx) const {
Matrix Cc = Matrix::Zero(3, 3 * __n_cal);
// If the measurement is related to a sensor that has a calibration state
if (idx >= 0) {
// Cc.block<3, 3>(0, 3 * idx) = wedge(d.d.unitVector()); // WRONG
// Set the correct 3x3 block in Cc
Cc.block<3, 3>(0, 3 * idx) = Rot3::Hat(d.d.unitVector());
}
Matrix3 wedge_d = Rot3::Hat(d.d.unitVector());
// This Matrix concatenation was different from the Python version
// Create the equivalent of:
// Rot3.Hat(d.d.unitVector()) @ np.hstack((Rot3.Hat(d.d.unitVector()), np.zeros((3, 3)), Cc))
Matrix temp(3, 6 + 3 * __n_cal);
temp.block<3, 3>(0, 0) = wedge_d;
temp.block<3, 3>(0, 3) = Matrix3::Zero();
temp.block(0, 6, 3, 3 * __n_cal) = Cc;
return wedge_d * temp;
}
Matrix EqF::__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>(__X_hat.B.size())) {
throw std::out_of_range("Calibration index out of range");
}
return __X_hat.B[idx].matrix();
} else {
return __X_hat.A.matrix();
}
}

153
examples/ABC_EQF/EqF.h
View File

@ -1,153 +0,0 @@
//
// Created by darshan on 3/11/25.
//
#ifndef EQF_H
#define EQF_H
#pragma once
#include "State.h"
#include "Input.h"
#include "G.h"
#include "Direction.h"
#include "Measurements.h"
#include <gtsam/base/Matrix.h>
using namespace gtsam;
/**
* Equivariant Filter (EqF) implementation
*/
class EqF {
private:
int __dof; // Degrees of freedom
int __n_cal; // Number of calibration states
int __n_sensor; // Number of sensors
G __X_hat; // Filter state
Matrix __Sigma; // Error covariance
State __xi_0; // Origin state
Matrix __Dphi0; // Differential of phi at origin
Matrix __InnovationLift; // Innovation lift matrix
public:
/**
* Initialize EqF
* @param Sigma Initial covariance
* @param n Number of calibration states
* @param m Number of sensors
*/
EqF(const Matrix& Sigma, int n, int m);
/**
* Return estimated state
* @return Current state estimate
*/
State 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);
private:
/**
* 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 Direction& d, int idx) const;
/**
* Return the measurement output matrix Dt
* @param idx Calibration index
* @return Measurement output matrix Dt
*/
Matrix __outputMatrixDt(int idx) const;
};
// Function declarations for helper functions used by EqF
/**
* Compute the lift of the system (Theorem 3.8, Equation 7)
* @param xi State
* @param u Input
* @return Lift vector
*/
Vector lift(const State& 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
*/
State stateAction(const G& X, const State& 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
*/
Input velocityAction(const G& 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
*/
Vector3 outputAction(const G& X, const Direction& y, int idx = -1);
/**
* Local coordinates assuming xi_0 = identity (Equation 9)
* @param e State representing equivariant error
* @return Local coordinates
*/
Vector local_coords(const State& e);
/**
* Local coordinates inverse assuming xi_0 = identity
* @param eps Local coordinates
* @return Corresponding state
*/
State local_coords_inv(const Vector& eps);
/**
* Differential of the phi action at E = Id in local coordinates
* @param xi State
* @return Differential matrix
*/
Matrix stateActionDiff(const State& xi);
#endif //EQF_H

61
examples/ABC_EQF/G.cpp
View File

@ -1,61 +0,0 @@
//
// Created by darshan on 3/11/25.
//
#include "G.h"
#include "utilities.h"
#include <stdexcept>
G::G(const Rot3& A, const Matrix3& a, const std::vector<Rot3>& B)
: A(A), a(a), B(B) {}
G G::operator*(const G& other) const {
if (B.size() != other.B.size()) {
throw std::invalid_argument("Group elements must have the same number of calibration elements");
}
std::vector<Rot3> new_B;
for (size_t i = 0; i < B.size(); i++) {
new_B.push_back(B[i] * other.B[i]);
}
return G(A * other.A,
a + Rot3::Hat(A.matrix() * Rot3::Vee(other.a)),
new_B);
}
G G::inv() const {
Matrix3 A_inv = A.inverse().matrix();
std::vector<Rot3> B_inv;
for (const auto& b : B) {
B_inv.push_back(b.inverse());
}
return G(A.inverse(),
-Rot3::Hat(A_inv * Rot3::Vee(a)),
B_inv);
}
G G::identity(int n) {
std::vector<Rot3> B(n, Rot3::Identity());
return G(Rot3::Identity(), Matrix3::Zero(), B);
}
G G::exp(const Vector& x) {
if (x.size() < 6 || x.size() % 3 != 0) {
throw std::invalid_argument("Wrong size, a vector with size multiple of 3 and at least 6 must be provided");
}
int n = (x.size() - 6) / 3;
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::vector<Rot3> B;
for (int i = 0; i < n; i++) {
B.push_back(Rot3::Expmap(x.segment<3>(6 + 3*i)));
}
return G(A, a, B);
}

63
examples/ABC_EQF/G.h
View File

@ -1,63 +0,0 @@
//
// Created by darshan on 3/11/25.
//
#ifndef G_H
#define G_H
#include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <vector>
using namespace gtsam;
/**
* 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
*/
class G {
public:
Rot3 A; // First SO(3) element
Matrix3 a; // so(3) element (skew-symmetric matrix)
std::vector<Rot3> B; // List of SO(3) elements for calibration
/**
* Initialize the symmetry group G
* @param A SO3 element
* @param a so(3) element (skew symmetric matrix)
* @param B list of SO3 elements
*/
G(const Rot3& A = Rot3::Identity(),
const Matrix3& a = Matrix3::Zero(),
const std::vector<Rot3>& B = std::vector<Rot3>());
/**
* Define the group operation (multiplication)
* @param other Another group element
* @return The product of this and other
*/
G operator*(const G& other) const;
/**
* Return the inverse element of the symmetry group
* @return The inverse of this group element
*/
G inv() const;
/**
* Return the identity of the symmetry group
* @param n Number of calibration elements
* @return The identity element with n calibration components
*/
static G identity(int n);
/**
* Return a group element X given by X = exp(x)
* @param x Vector representation of Lie algebra element
* @return Group element given by the exponential of x
*/
static G exp(const Vector& x);
};
#endif //G_H

30
examples/ABC_EQF/Input.cpp
View File

@ -1,30 +0,0 @@
//
// Created by darshan on 3/11/25.
//
#include "Input.h"
#include "utilities.h"
#include <Eigen/Dense>
#include <stdexcept>
#include "gtsam/geometry/Rot3.h"
Input::Input(const Vector3& w, const Matrix& Sigma)
: w(w), Sigma(Sigma) {
if (Sigma.rows() != 6 || Sigma.cols() != 6) {
throw std::invalid_argument("Input measurement noise covariance must be 6x6");
}
// 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");
}
}
Matrix3 Input::W() const {
return Rot3::Hat(w);
}
Input Input::random() {
Vector3 w = Vector3::Random();
return Input(w, Matrix::Identity(6, 6));
}

41
examples/ABC_EQF/Input.h
View File

@ -1,41 +0,0 @@
//
// Created by darshan on 3/11/25.
//
#ifndef INPUT_H
#define INPUT_H
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
using namespace gtsam;
/**
* Input class for the Biased Attitude System
*/
class Input {
public:
Vector3 w; // Angular velocity
Matrix Sigma; // Noise covariance
/**
* Initialize Input
* @param w Angular velocity (3-vector)
* @param Sigma Noise covariance (6x6 matrix)
*/
Input(const Vector3& w, const Matrix& Sigma);
/**
* Return the Input as a skew-symmetric matrix
* @return w as a skew-symmetric matrix
*/
Matrix3 W() const;
/**
* Return a random angular velocity
* @return A random angular velocity as Input element
*/
static Input random();
};
#endif //INPUT_H

17
examples/ABC_EQF/Measurements.cpp
View File

@ -1,17 +0,0 @@
//
// Created by darshan on 3/11/25.
//
#include "Measurements.h"
#include <Eigen/Dense>
#include <stdexcept>
Measurement::Measurement(const Vector3& y_vec, const Vector3& d_vec,
const Matrix3& Sigma, int i)
: y(y_vec), d(d_vec), Sigma(Sigma), cal_idx(i) {
// Check positive semi-definite
Eigen::SelfAdjointEigenSolver<Matrix3> eigensolver(Sigma);
if (eigensolver.eigenvalues().minCoeff() < -1e-10) {
throw std::invalid_argument("Covariance matrix must be semi-positive definite");
}
}

36
examples/ABC_EQF/Measurements.h
View File

@ -1,36 +0,0 @@
//
// Created by darshan on 3/11/25.
//
#ifndef MEASUREMENTS_H
#define MEASUREMENTS_H
#include "Direction.h"
#include <gtsam/base/Matrix.h>
using namespace gtsam;
/**
* Measurement class
* cal_idx is an index corresponding to the calibration related to the measurement
* cal_idx = -1 indicates the measurement is from a calibrated sensor
*/
class Measurement {
public:
Direction y; // Measurement direction in sensor frame
Direction d; // Known direction in global frame
Matrix3 Sigma; // Covariance matrix of the measurement
int cal_idx = -1; // Calibration index (-1 for calibrated sensor)
/**
* Initialize measurement
* @param y_vec Direction measurement in sensor frame
* @param d_vec Known direction in global frame
* @param Sigma Measurement noise covariance
* @param i Calibration index (-1 for calibrated sensor)
*/
Measurement(const Vector3& y_vec, const Vector3& d_vec,
const Matrix3& Sigma, int i = -1);
};
#endif //MEASUREMENTS_H

12
examples/ABC_EQF/State.cpp
View File

@ -1,12 +0,0 @@
//
// Created by darshan on 3/11/25.
//
#include "State.h"
State::State(const Rot3& R, const Vector3& b, const std::vector<Rot3>& S)
: R(R), b(b), S(S) {}
State State::identity(int n) {
std::vector<Rot3> calibrations(n, Rot3::Identity());
return State(Rot3::Identity(), Vector3::Zero(), calibrations);
}

29
examples/ABC_EQF/State.h
View File

@ -1,29 +0,0 @@
//
// Created by darshan on 3/11/25.
//
#ifndef STATE_H
#define STATE_H
#include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Vector.h>
#include <vector>
using namespace gtsam;
/**
* State class representing the state of the Biased Attitude System
*/
class State {
public:
Rot3 R; // Attitude rotation matrix R
Vector3 b; // Gyroscope bias b
std::vector<Rot3> S; // Sensor calibrations S
State(const Rot3& R = Rot3::Identity(),
const Vector3& b = Vector3::Zero(),
const std::vector<Rot3>& S = std::vector<Rot3>());
static State identity(int n);
};
#endif //STATE_H

194
examples/ABC_EQF/main.cpp
View File

@ -1,194 +0,0 @@
//
// Created by darshan on 3/11/25.
//
#include "EqF.h"
#include "State.h"
#include "Input.h"
#include "Direction.h"
#include "Measurements.h"
#include "Data.h"
#include "runEQF_withcsv.h"
#include "utilities.h"
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <chrono>
#include <cmath>
#include <gtsam/slam/dataset.h>
using namespace std;
using namespace gtsam;
// Simplified data loading function - in a real application, implement proper CSV parsing
std::vector<Data> loadSimulatedData() {
std::vector<Data> data_list;
double t = 0.0;
double dt = 0.01;
// Number of data points
int num_points = 100;
// Set up one calibration state
int n_cal = 1;
for (int i = 0; i < num_points; i++) {
t += dt;
// Create a simple sinusoidal trajectory
double angle = 0.1 * sin(t);
Rot3 R = Rot3::Rz(angle);
// Create a bias
Vector3 b(0.01, 0.02, 0.03);
// Create a calibration
std::vector<Rot3> S;
S.push_back(Rot3::Ry(0.05));
// State
State xi(R, b, S);
// Input (angular velocity)
Vector3 w(0.1 * cos(t), 0.05 * sin(t), 0.02);
Matrix Sigma_u = Matrix::Identity(6, 6) * 0.01;
Input u(w, Sigma_u);
// Measurements
std::vector<Measurement> measurements;
// Measurement 1 - from uncalibrated sensor
Vector3 d1_vec = Vector3(1, 0, 0).normalized(); // Known direction in global frame
Vector3 y1_vec = S[0].inverse().matrix() * R.inverse().matrix() * d1_vec; // Direction in sensor frame
Matrix3 Sigma1 = Matrix3::Identity() * 0.01;
measurements.push_back(Measurement(y1_vec, d1_vec, Sigma1, 0)); // cal_idx = 0
// Measurement 2 - from calibrated sensor
Vector3 d2_vec = Vector3(0, 1, 0).normalized(); // Known direction in global frame
Vector3 y2_vec = R.inverse().matrix() * d2_vec; // Direction in sensor frame
Matrix3 Sigma2 = Matrix3::Identity() * 0.01;
measurements.push_back(Measurement(y2_vec, d2_vec, Sigma2, -1)); // cal_idx = -1
// Add to data list
data_list.push_back(Data(xi, n_cal, u, measurements, 2, t, dt));
}
return data_list;
}
void runSimulation(EqF& filter, const std::vector<Data>& data) {
std::cout << "Starting simulation with " << data.size() << " data points..." << std::endl;
// Track time for performance measurement
auto start = std::chrono::high_resolution_clock::now();
// Store results for analysis
std::vector<double> times;
std::vector<Vector3> attitude_errors;
std::vector<Vector3> bias_errors;
std::vector<Vector3> calibration_errors;
for (const auto& d : data) {
// Propagation
try {
filter.propagation(d.u, d.dt);
} catch (const std::exception& e) {
std::cerr << "Propagation error at t=" << d.t << ": " << e.what() << std::endl;
continue;
}
// Update with measurements
for (const auto& y : d.y) {
try {
if (!std::isnan(y.y.d.unitVector().norm()) && !std::isnan(y.d.d.unitVector().norm())) {
filter.update(y);
}
} catch (const std::exception& e) {
std::cerr << "Update error at t=" << d.t << ": " << e.what() << std::endl;
}
}
// Get state estimate
State estimate = filter.stateEstimate();
// Compute errors
Vector3 att_error = Rot3::Logmap(d.xi.R.between(estimate.R));
Vector3 bias_error = estimate.b - d.xi.b;
Vector3 cal_error = Vector3::Zero();
if (!d.xi.S.empty() && !estimate.S.empty()) {
cal_error = Rot3::Logmap(d.xi.S[0].between(estimate.S[0]));
}
// Store results
times.push_back(d.t);
attitude_errors.push_back(att_error);
bias_errors.push_back(bias_error);
calibration_errors.push_back(cal_error);
// Print some info
if (d.t < 0.1 || fmod(d.t, 1.0) < d.dt) {
std::cout << "Time: " << d.t
<< ", Attitude error (deg): " << (att_error.norm() * 180.0/M_PI)
<< ", Bias error: " << bias_error.norm()
<< ", Calibration error (deg): " << (cal_error.norm() * 180.0/M_PI)
<< std::endl;
}
}
auto end = std::chrono::high_resolution_clock::now();
std::chrono::duration<double> elapsed = end - start;
std::cout << "Simulation completed in " << elapsed.count() << " seconds" << std::endl;
// Print summary statistics
double avg_att_error = 0.0;
double avg_bias_error = 0.0;
double avg_cal_error = 0.0;
for (size_t i = 0; i < times.size(); i++) {
avg_att_error += attitude_errors[i].norm();
avg_bias_error += bias_errors[i].norm();
avg_cal_error += calibration_errors[i].norm();
}
avg_att_error /= times.size();
avg_bias_error /= times.size();
avg_cal_error /= times.size();
std::cout << "Average attitude error (deg): " << (avg_att_error * 180.0/M_PI) << std::endl;
std::cout << "Average bias error: " << avg_bias_error << std::endl;
std::cout << "Average calibration error (deg): " << (avg_cal_error * 180.0/M_PI) << std::endl;
}
int main(int argc, char** argv) {
std::cout << "ABC-EqF: Attitude-Bias-Calibration Equivariant Filter" << std::endl;
std::cout << "========================================================" << std::endl;
std::string csvFilePath;
// Try to find the EQFdata file in the GTSAM examples directory
try {
// Look specifically for EQFdata.csv in GTSAM examples
csvFilePath = findExampleDataFile("EqFdata.csv");
std::cout << "Using GTSAM example data file: " << csvFilePath << std::endl;
} catch (const std::exception& e) {
std::cerr << "Error: Could not find EqFdata.csv in GTSAM examples directory" << std::endl;
std::cerr << e.what() << std::endl;
return 1;
}
try {
// Run with CSV data
runEqFWithCSVData(csvFilePath);
} catch (const std::exception& e) {
std::cerr << "Error: " << e.what() << std::endl;
return 1;
}
std::cout << "Done." << std::endl;
return 0;
}

683
examples/ABC_EQF/runEQF_withcsv.h
View File

@ -1,683 +0,0 @@
//
// Created by darshan on 3/17/25.
//
#ifndef RUNEQF_WITHCSV_H
#define RUNEQF_WITHCSV_H
//
// Created by darshan on 3/17/25.
//
#include "Data.h"
#include "State.h"
#include "Input.h"
#include "Direction.h"
#include "Measurements.h"
#include "utilities.h"
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Quaternion.h>
#include <fstream>
#include <sstream>
#include <string>
#include <vector>
#include <stdexcept>
#include <iostream>
#include <cmath>
#include <chrono>
/**
* 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
*
* @param filename Path to the CSV file
* @param startRow First row to load (default: 0)
* @param maxRows Maximum number of rows to load (default: all)
* @param downsample Downsample factor (default: 1, which means no downsampling)
* @return Vector of Data objects
*/
inline std::vector<Data> loadDataFromCSV(const std::string& filename,
int startRow = 0,
int maxRows = -1,
int downsample = 1) {
std::vector<Data> data_list;
std::ifstream file(filename);
if (!file.is_open()) {
throw std::runtime_error("Failed to open file: " + filename);
}
std::string line;
int lineNumber = 0;
int rowCount = 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) {
std::cerr << "Error parsing value at line " << lineNumber << ": " << token << std::endl;
values.push_back(0.0); // Use default value
}
}
// Check if we have enough values
if (values.size() < 39) {
std::cerr << "Warning: Line " << lineNumber << " has only " << values.size()
<< " values, expected 39. Skipping." << std::endl;
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::vector<Rot3> S = {Rot3(calQuat)};
State 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(y0, 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(y1, d1, covY1, -1));
// Create Data object and add to list
data_list.push_back(Data(xi, 1, u, measurements, 2, t, dt));
rowCount++;
}
std::cout << "Loaded " << data_list.size() << " data points from CSV file." << std::endl;
return data_list;
}
/**
* Process Data objects with the EqF filter
*
* @param filter EqF filter to use
* @param data_list Vector of Data objects
* @param saveResults Whether to save results to a file
* @param resultFilename Filename to save results to
*/
inline void printDataPoint(const Data& data, int index) {
std::cout << "Data[" << index << "] @ t=" << data.t << ", dt=" << data.dt << std::endl;
// Print angular velocity
std::cout << " ω = [" << data.u.w[0] << ", " << data.u.w[1] << ", " << data.u.w[2] << "]" << std::endl;
// Print measurements
for (size_t i = 0; i < data.y.size(); i++) {
const Measurement& meas = data.y[i];
// Use the unitVector() method to get a Vector3 from a Unit3 object
Vector3 y_vec = meas.y.d.unitVector();
Vector3 d_vec = meas.d.d.unitVector();
std::cout << " y" << i << " = [" << y_vec[0] << ", " << y_vec[1] << ", " << y_vec[2] << "]" << std::endl;
std::cout << " d" << i << " = [" << d_vec[0] << ", " << d_vec[1] << ", " << d_vec[2] << "]" << std::endl;
}
std::cout << std::endl;
}
// Function to print sample data points
inline void printDataSamples(const std::vector<Data>& data_list, int count = 3) {
int total = data_list.size();
std::cout << "\n=== First " << count << " Data Points ===" << std::endl;
for (int i = 0; i < std::min(count, total); i++) {
printDataPoint(data_list[i], i);
}
if (total > 2*count) {
std::cout << "\n... (" << (total - 2*count) << " points omitted) ...\n" << std::endl;
std::cout << "=== Last " << count << " Data Points ===" << std::endl;
for (int i = std::max(count, total - count); i < total; i++) {
printDataPoint(data_list[i], i);
}
}
}
// Function to validate data
inline bool validateData(const std::vector<Data>& data_list) {
if (data_list.empty()) {
std::cerr << "ERROR: No data loaded from CSV" << std::endl;
return false;
}
std::cout << "Validating " << data_list.size() << " data points..." << std::endl;
// Track statistics
int invalid_count = 0;
// Open a log file to record detailed issues
std::ofstream logFile("data_validation.log");
logFile << "Data Validation Report" << std::endl;
logFile << "--------------------" << std::endl;
for (size_t i = 0; i < data_list.size(); ++i) {
const Data& data = data_list[i];
bool point_valid = true;
// Check time and dt
if (std::isnan(data.t) || std::isnan(data.dt)) {
logFile << "Point " << i << ": Invalid time values (t=" << data.t
<< ", dt=" << data.dt << ")" << std::endl;
point_valid = false;
}
// Check ground truth state for NaN - using isnan directly on components
const auto& R_matrix = data.xi.R.matrix();
bool R_has_nan = false;
for (int r = 0; r < 3; r++) {
for (int c = 0; c < 3; c++) {
if (std::isnan(R_matrix(r, c))) {
R_has_nan = true;
break;
}
}
}
if (R_has_nan) {
logFile << "Point " << i << ": NaN in ground truth attitude matrix" << std::endl;
point_valid = false;
}
// Check bias vector for NaN
bool b_has_nan = false;
for (int j = 0; j < 3; j++) {
if (std::isnan(data.xi.b[j])) {
b_has_nan = true;
break;
}
}
if (b_has_nan) {
logFile << "Point " << i << ": NaN in ground truth bias vector" << std::endl;
point_valid = false;
}
// Check calibration matrices for NaN
for (size_t j = 0; j < data.xi.S.size(); ++j) {
const auto& S_matrix = data.xi.S[j].matrix();
bool S_has_nan = false;
for (int r = 0; r < 3; r++) {
for (int c = 0; c < 3; c++) {
if (std::isnan(S_matrix(r, c))) {
S_has_nan = true;
break;
}
}
}
if (S_has_nan) {
logFile << "Point " << i << ": NaN in ground truth calibration matrix "
<< j << std::endl;
point_valid = false;
}
}
// Check input for NaN
bool w_has_nan = false;
for (int j = 0; j < 3; j++) {
if (std::isnan(data.u.w[j])) {
w_has_nan = true;
break;
}
}
if (w_has_nan) {
logFile << "Point " << i << ": NaN in angular velocity" << std::endl;
point_valid = false;
}
// Check measurements
for (size_t j = 0; j < data.y.size(); ++j) {
const Measurement& meas = data.y[j];
// Get the Vector3 representations to check them
Vector3 y_vec = meas.y.d.unitVector();
Vector3 d_vec = meas.d.d.unitVector();
// Check measurement vector for NaN
bool y_has_nan = false;
bool d_has_nan = false;
for (int k = 0; k < 3; k++) {
if (std::isnan(y_vec[k])) {
y_has_nan = true;
break;
}
if (std::isnan(d_vec[k])) {
d_has_nan = true;
break;
}
}
if (y_has_nan) {
logFile << "Point " << i << ", Meas " << j << ": NaN in measurement vector" << std::endl;
point_valid = false;
}
if (d_has_nan) {
logFile << "Point " << i << ", Meas " << j << ": NaN in reference direction" << std::endl;
point_valid = false;
}
// Calculate norm using Vector3 norms
double y_norm = y_vec.norm();
double d_norm = d_vec.norm();
if (std::abs(y_norm - 1.0) > 1e-5) {
logFile << "Point " << i << ", Meas " << j
<< ": Measurement vector not normalized. Norm = " << y_norm << std::endl;
point_valid = false;
}
if (std::abs(d_norm - 1.0) > 1e-5) {
logFile << "Point " << i << ", Meas " << j
<< ": Reference direction not normalized. Norm = " << d_norm << std::endl;
point_valid = false;
}
}
if (!point_valid) {
invalid_count++;
// Print first few invalid points to console
if (invalid_count <= 5) {
std::cerr << "Invalid data at point " << i << " (t=" << data.t << ")" << std::endl;
}
}
}
// Close the log
logFile << std::endl << "Summary: " << invalid_count << " invalid data points out of "
<< data_list.size() << std::endl;
logFile.close();
// Print summary
std::cout << "Data validation complete. " << invalid_count << " invalid points found." << std::endl;
if (invalid_count > 0) {
std::cout << "See data_validation.log for details." << std::endl;
}
return (invalid_count == 0);
}
inline void processDataWithEqF(EqF& filter,
const std::vector<Data>& data_list,
bool saveResults = false,
const std::string& resultFilename = "eqf_results.csv") {
std::ofstream resultFile;
if (saveResults) {
resultFile.open(resultFilename);
if (!resultFile.is_open()) {
throw std::runtime_error("Failed to open result file: " + resultFilename);
}
// Write header - now adding roll, pitch, yaw columns for estimated and true values
resultFile << "t,";
// Estimated state quaternion
resultFile << "est_qw,est_qx,est_qy,est_qz,";
// Estimated bias
resultFile << "est_bx,est_by,est_bz,";
// Estimated calibration quaternion
resultFile << "est_cqw,est_cqx,est_cqy,est_cqz,";
// True state quaternion
resultFile << "true_qw,true_qx,true_qy,true_qz,";
// True bias
resultFile << "true_bx,true_by,true_bz,";
// True calibration quaternion
resultFile << "true_cqw,true_cqx,true_cqy,true_cqz,";
// Add Euler angles for estimated state
resultFile << "est_roll,est_pitch,est_yaw,";
// Add Euler angles for true state
resultFile << "true_roll,true_pitch,true_yaw,";
// Add Euler angles for estimated calibration
resultFile << "est_cal_roll,est_cal_pitch,est_cal_yaw,";
// Add Euler angles for true calibration
resultFile << "true_cal_roll,true_cal_pitch,true_cal_yaw";
resultFile << std::endl;
}
std::cout << "Processing data with EqF..." << std::endl;
// Track time for performance measurement
auto start = std::chrono::high_resolution_clock::now();
// Store error metrics
std::vector<double> att_errors;
std::vector<double> bias_errors;
std::vector<double> cal_errors;
int total_measurements = 0;
int valid_measurements = 0;
int invalid_measurements = 0;
for (size_t i = 0; i < data_list.size(); i++) {
const Data& data = data_list[i];
// Propagation
filter.propagation(data.u, data.dt);
// Update with measurements
for (const auto& y : data.y) {
total_measurements++;
// Check for NaN values in measurement vectors
bool has_nan = false;
Vector3 y_vec = y.y.d.unitVector();
Vector3 d_vec = y.d.d.unitVector();
for (int j = 0; j < 3; j++) {
if (std::isnan(y_vec[j]) || std::isnan(d_vec[j])) {
has_nan = true;
break;
}
}
if (!has_nan) {
try {
filter.update(y);
valid_measurements++;
} catch (const std::exception& e) {
std::cerr << "Error updating at t=" << data.t << ": " << e.what() << std::endl;
invalid_measurements++;
}
} else {
invalid_measurements++;
}
}
// Get state estimate
State estimate = filter.stateEstimate();
// Compute 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());
// Save results
if (saveResults) {
// Extract quaternions
Quaternion est_q = estimate.R.toQuaternion();
Quaternion true_q = data.xi.R.toQuaternion();
// Extract Euler angles (roll, pitch, yaw) from estimated rotation
Vector3 est_rpy = estimate.R.rpy();
// Convert to degrees for easier comparison
Vector3 est_rpy_deg = est_rpy * 180.0 / M_PI;
// Extract Euler angles from true rotation
Vector3 true_rpy = data.xi.R.rpy();
// Convert to degrees
Vector3 true_rpy_deg = true_rpy * 180.0 / M_PI;
// Get calibration quaternions and Euler angles
Quaternion est_cal_q, true_cal_q;
Vector3 est_cal_rpy_deg = Vector3::Zero();
Vector3 true_cal_rpy_deg = Vector3::Zero();
if (!estimate.S.empty() && !data.xi.S.empty()) {
est_cal_q = estimate.S[0].toQuaternion();
true_cal_q = data.xi.S[0].toQuaternion();
// Get Euler angles for calibrations
Vector3 est_cal_rpy = estimate.S[0].rpy();
est_cal_rpy_deg = est_cal_rpy * 180.0 / M_PI;
Vector3 true_cal_rpy = data.xi.S[0].rpy();
true_cal_rpy_deg = true_cal_rpy * 180.0 / M_PI;
} else {
est_cal_q = Quaternion(1, 0, 0, 0); // Identity quaternion
true_cal_q = Quaternion(1, 0, 0, 0);
}
// Write to file
resultFile << data.t << ",";
// Estimated quaternion
resultFile << est_q.w() << "," << est_q.x() << "," << est_q.y() << "," << est_q.z() << ",";
// Estimated bias
resultFile << estimate.b[0] << "," << estimate.b[1] << "," << estimate.b[2] << ",";
// Estimated calibration quaternion
resultFile << est_cal_q.w() << "," << est_cal_q.x() << "," << est_cal_q.y() << "," << est_cal_q.z() << ",";
// True quaternion
resultFile << true_q.w() << "," << true_q.x() << "," << true_q.y() << "," << true_q.z() << ",";
// True bias
resultFile << data.xi.b[0] << "," << data.xi.b[1] << "," << data.xi.b[2] << ",";
// True calibration quaternion
resultFile << true_cal_q.w() << "," << true_cal_q.x() << "," << true_cal_q.y() << "," << true_cal_q.z() << ",";
// Add Euler angles (in degrees) for estimated state
resultFile << est_rpy_deg[0] << "," << est_rpy_deg[1] << "," << est_rpy_deg[2] << ",";
// Add Euler angles (in degrees) for true state
resultFile << true_rpy_deg[0] << "," << true_rpy_deg[1] << "," << true_rpy_deg[2] << ",";
// Add Euler angles (in degrees) for estimated calibration
resultFile << est_cal_rpy_deg[0] << "," << est_cal_rpy_deg[1] << "," << est_cal_rpy_deg[2] << ",";
// Add Euler angles (in degrees) for true calibration
resultFile << true_cal_rpy_deg[0] << "," << true_cal_rpy_deg[1] << "," << true_cal_rpy_deg[2];
resultFile << std::endl;
}
// Print progress
if (i % 1000 == 0 || i == data_list.size() - 1) {
std::cout << "Processed " << i+1 << "/" << data_list.size()
<< " (" << (100.0 * (i+1) / data_list.size()) << "%) ";
std::cout << "Attitude error: " << (att_error.norm() * 180.0/M_PI) << " deg, ";
std::cout << "Bias error: " << bias_error.norm() << ", ";
std::cout << "Calibration error: " << (cal_error.norm() * 180.0/M_PI) << " deg" << 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();
}
std::cout << std::endl;
std::cout << "EqF Processing completed in " << elapsed.count() << " seconds" << std::endl;
std::cout << "Average attitude error: " << (avg_att_error * 180.0/M_PI) << " deg" << std::endl;
std::cout << "Average bias error: " << avg_bias_error << std::endl;
std::cout << "Average calibration error: " << (avg_cal_error * 180.0/M_PI) << " deg" << std::endl;
std::cout << "Total measurements: " << total_measurements << std::endl;
std::cout << "Valid measurements processed: " << valid_measurements << std::endl;
std::cout << "Invalid measurements skipped: " << invalid_measurements << std::endl;
if (saveResults) {
resultFile.close();
std::cout << "Results saved to " << resultFilename << std::endl;
}
}
inline void runEqFWithCSVData(const std::string& filename) {
try {
// Load data from CSV file with optional parameters
int startRow = 0;
int maxRows = -1;
int downsample = 1;
std::vector<Data> data = loadDataFromCSV(filename, startRow, maxRows, downsample);
if (data.empty()) {
std::cerr << "No data loaded from CSV file." << std::endl;
return;
}
// Print sample data points to inspect the loaded data
std::cout << "Data loaded, displaying samples..." << std::endl;
printDataSamples(data);
// Validate the data to check for issues
std::cout << "Validating data integrity..." << std::endl;
bool dataValid = validateData(data);
if (!dataValid) {
std::cout << "Warning: Data validation found issues." << std::endl;
std::string proceed;
std::cout << "Do you want to proceed anyway? (y/n): ";
std::cin >> proceed;
if (proceed != "y" && proceed != "Y") {
std::cout << "Operation cancelled by user." << std::endl;
return;
}
}
// Initialize EqF filter
int n_cal = 1; // Number of calibration states (from the data)
int n_sensors = 2; // Number of sensors (from the data)
// Initial covariance
Matrix initialSigma = Matrix::Identity(6 + 3*n_cal, 6 + 3*n_cal);
initialSigma.diagonal().head<3>() = Vector3::Constant(0.1); // Reduced attitude uncertainty
initialSigma.diagonal().segment<3>(3) = Vector3::Constant(0.01); // Reduced bias uncertainty
initialSigma.diagonal().tail<3>() = Vector3::Constant(0.1); // Reduced calibration uncertainty
// Create filter
EqF filter(initialSigma, n_cal, n_sensors);
// Initialize filter state from the first ground truth if possible
if (!data.empty()) {
// You'll need to add a method to your EqF class to set the initial state
// Something like:
// filter.setInitialState(data[0].xi);
// If you don't have such a method, you can print the first ground truth
// to see if it makes sense
std::cout << "First ground truth state:" << std::endl;
std::cout << "Attitude: " << data[0].xi.R.matrix() << std::endl;
std::cout << "Bias: " << data[0].xi.b.transpose() << std::endl;
std::cout << "Calibration: " << data[0].xi.S[0].matrix() << std::endl;
}
// Process data with the filter and save results
processDataWithEqF(filter, data, true, "eqf_results.csv");
} catch (const std::exception& e) {
std::cerr << "Error: " << e.what() << std::endl;
}
}
/**
* Example usage function to demonstrate how to use the data loader with the EqF
*/
#endif //RUNEQF_WITHCSV_H

57
examples/ABC_EQF/utilities.cpp
View File

@ -1,57 +0,0 @@
//
// Created by darshan on 3/11/25.
//
#include "utilities.h"
#include <cmath>
// Global configuration
const std::string COORDINATE = "EXPONENTIAL"; // Alternative: "NORMAL"
bool checkNorm(const Vector3& x, double tol) {
return abs(x.norm() - 1) < tol || std::isnan(x.norm());
}
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;
}
}
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;
}
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;
}

28
examples/ABC_EQF/utilities.h
View File

@ -1,28 +0,0 @@
//
// Created by darshan on 3/11/25.
//
#ifndef UTILITIES_H
#define UTILITIES_H
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <Eigen/Dense>
#include <functional>
using namespace gtsam;
// Global configuration
extern const std::string COORDINATE; // "EXPONENTIAL" or "NORMAL"
/**
* Utility functions
*/
Matrix3 wedge(const Vector3& vec);
bool checkNorm(const Vector3& x, double tol = 1e-3);
Matrix blockDiag(const Matrix& A, const Matrix& B);
Matrix repBlock(const Matrix& A, int n);
Matrix numericalDifferential(std::function<Vector(const Vector&)> f, const Vector& x);
#endif //UTILITIES_H

89
examples/ABC_EQF_Demo.cpp Normal file
View File

@ -0,0 +1,89 @@
/**
* @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 abc_eqf_lib;
using namespace gtsam;
/**
* Main function for the EqF demo
* @param argc Number of arguments
* @param argv Array of arguments
* @return Exit code
*/
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_cal = 1;
int n_sensors = 2;
// Initial covariance - larger values allow faster convergence
Matrix initialSigma = Matrix::Identity(6 + 3*n_cal, 6 + 3*n_cal);
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
EqF filter(initialSigma, n_cal, 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;
}

1
examples/CMakeLists.txt
View File

@ -17,4 +17,3 @@ if (NOT GTSAM_USE_BOOST_FEATURES)
endif()
gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;${Boost_PROGRAM_OPTIONS_LIBRARY}" ${GTSAM_BUILD_EXAMPLES_ALWAYS})
add_subdirectory(ABC_EQF)

5
gtsam/CMakeLists.txt
View File

@ -86,7 +86,10 @@ endforeach(subdir)
# To add additional sources to gtsam when building the full library (static or shared)
# append the subfolder with _srcs appended to the end to this list
set(gtsam_srcs ${3rdparty_srcs})
set(gtsam_srcs ${3rdparty_srcs}
../examples/ABC_EQF_Demo.cpp
../examples/ABC_EQF.cpp
../examples/ABC_EQF.h)
foreach(subdir ${gtsam_subdirs})
list(APPEND gtsam_srcs ${${subdir}_srcs})
endforeach(subdir)