From 17bf752576c33d2b28c0c30140c3eb7274dad526 Mon Sep 17 00:00:00 2001 From: jenniferoum Date: Fri, 25 Apr 2025 07:08:14 -0700 Subject: [PATCH] Template on size_t, Darshan's updates to cleanup comments, default coordinate to exponential, separate filter and demo specific functions, rename stateAction to operator *, fix brace initialization --- examples/ABC.h | 259 +++++++++++ examples/ABC_EQF.h | 872 ++++++++++++-------------------------- examples/ABC_EQF_Demo.cpp | 736 ++++++++++++++++---------------- 3 files changed, 895 insertions(+), 972 deletions(-) create mode 100644 examples/ABC.h diff --git a/examples/ABC.h b/examples/ABC.h new file mode 100644 index 000000000..a7c029c98 --- /dev/null +++ b/examples/ABC.h @@ -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 +#include +#include +#include + +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 +class State { + public: + Rot3 R; // Attitude rotation matrix R + Vector3 b; // Gyroscope bias b + std::array S; // Sensor calibrations S + + /// Constructor + State(const Rot3& R = Rot3::Identity(), const Vector3& b = Vector3::Zero(), + const std::array& S = std::array{}) + : R(R), b(b), S(S) {} + + /// Identity function + static State identity() { + std::array 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& 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(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 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 +struct G { + Rot3 A; /// First SO(3) element + Matrix3 a; /// so(3) element (skew-symmetric matrix) + std::array 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& B = std::array{}) + : A(A), a(a), B(B) {} + + /// Group multiplication + G operator*(const G& other) const { + std::array 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 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 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(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 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 +struct traits> + : internal::LieGroupTraits> {}; + +template +struct traits> : internal::LieGroupTraits> { +}; + +} // namespace gtsam + +#endif // ABC_H diff --git a/examples/ABC_EQF.h b/examples/ABC_EQF.h index 4c935962c..02b8dd4b9 100644 --- a/examples/ABC_EQF.h +++ b/examples/ABC_EQF.h @@ -2,290 +2,60 @@ * @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 + * 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 #include #include #include -#include #include -#include #include +#include +#include -#include +#include +#include #include +#include +#include +#include // For std::accumulate #include #include -#include -#include -#include -#include // For std::accumulate + +#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; -// 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 f, const Vector& x); - -//======================================================================== -// 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); - } - static Input create(const Vector3& w, const Matrix& Sigma) { /// Initialize w and 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 eigensolver(Sigma); - if (eigensolver.eigenvalues().minCoeff() < -1e-10) { - throw std::invalid_argument("Covariance matrix must be semi-positive definite"); - } - - return Input{w, Sigma}; // use brace initialization - } -}; - -/// 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) - static Measurement create(const Unit3& y_vec, const Unit3& d_vec, /// Initialize measurement - const Matrix3& Sigma_in, int i) { - /// Check positive semi-definite - Eigen::SelfAdjointEigenSolver eigensolver(Sigma_in); - if (eigensolver.eigenvalues().minCoeff() < -1e-10) { - throw std::invalid_argument("Covariance matrix must be semi-positive definite"); - } - return Measurement{y_vec, Unit3(d_vec), Sigma_in, i}; // Brace initialization - } -}; - -/// 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 S; // Sensor calibrations S - - /// Constructor - State(const Rot3& R = Rot3::Identity(), - const Vector3& b = Vector3::Zero(), - const std::vector& S = std::vector()) - : R(R), b(b), S(S) {} - - /// Identity function - static State identity(int n) { - std::vector calibrations(n, Rot3::Identity()); - return State(Rot3::Identity(), Vector3::Zero(), calibrations); - } - /** - * 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& other) const { - Vector eps(6 + 3 * S.size()); - - // 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 < S.size(); 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() < 6 || v.size() % 3 != 0 || v.size() != 6 + 3 * static_cast(S.size())) { - throw std::invalid_argument("Vector size does not match state dimensions"); - } - - // Modify attitude - Rot3 newR = R * Rot3::Expmap(v.head<3>()); - - // Modify bias - Vector3 newB = b + v.segment<3>(3); - - // Modify calibrations - std::vector newS; - for (size_t i = 0; i < S.size(); i++) { - newS.push_back(S[i] * Rot3::Expmap(v.segment<3>(6 + 3*i))); - } - - return State(newR, newB, newS); - } -}; - -/// 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 y; /// Output measurements - int n_meas; /// Number of measurements - double t; /// Time - double dt; /// Time step - - static Data create(const State& xi, int n_cal, const Input& u, /// Initialize Data - const std::vector& y, int n_meas, - double t, double dt) { - return Data{xi, n_cal, u, y, n_meas, t, dt}; /// Bracket initialization - } -}; - -//======================================================================== -// 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 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::vector& B = std::vector()) - : A(A), a(a), B(B) {} - - /// Group multiplication - 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 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); - } - - /// Group inverse - G inv() const { - Matrix3 A_inv = A.inverse().matrix(); - std::vector 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); - } - - /// Identity element - static G identity(int n) { - std::vector B(n, 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() < 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 B; - for (int i = 0; i < n; i++) { - B.push_back(Rot3::Expmap(x.segment<3>(6 + 3 * i))); - } - - return G(A, a, B); - } -}; - //======================================================================== // Helper Functions for EqF //======================================================================== +/// Calculate numerical differential + +Matrix numericalDifferential(std::function f, + const Vector& x); + /** * 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); +template +Vector lift(const State& xi, const Input& u); /** * Action of the symmetry group on the state space (Equation 4) @@ -293,7 +63,8 @@ Vector lift(const State& xi, const Input& u); * @param xi State * @return New state after group action */ -State stateAction(const G& X, const State& xi); +template +State operator*(const G& X, const State& xi); /** * Action of the symmetry group on the input space (Equation 5) @@ -301,7 +72,8 @@ State stateAction(const G& X, const State& xi); * @param u Input * @return New input after group action */ -Input velocityAction(const G& X, const Input& u); +template +Input velocityAction(const G& X, const Input& u); /** * Action of the symmetry group on the output space (Equation 6) @@ -310,188 +82,96 @@ Input velocityAction(const G& X, const Input& u); * @param idx Calibration index * @return New direction after group action */ -Vector3 outputAction(const G& X, const Unit3& y, int idx); +template +Vector3 outputAction(const G& X, const Unit3& y, int idx); /** * Differential of the phi action at E = Id in local coordinates * @param xi State * @return Differential matrix */ -Matrix stateActionDiff(const State& xi); +template +Matrix stateActionDiff(const State& xi); //======================================================================== // Equivariant Filter (EqF) //======================================================================== /// Equivariant Filter (EqF) implementation - +template class EqF { -private: - int dof; // Degrees of freedom - int n_cal; // Number of calibration states - 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 + private: + int dof; // Degrees of freedom + 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 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 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 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 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; + /** + * 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); + 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 stateEstimate() const; + /** + * 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); + /** + * 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); + /** + * Update the filter state with a measurement + * @param y Direction measurement + */ + void update(const Measurement& y); }; -// Global configuration -const std::string COORDINATE = "EXPONENTIAL"; // Denotes how the states are mapped to the vector representations - -//======================================================================== -// Utility Functions -//======================================================================== -/** - * @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 - * Uses Vector3 norm() method to calculate vector magnitude - */ -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 - * Uses Matrix's rows(), cols(), setZero(), and block() methods - */ - -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 - * Recursively uses blockDiag() function - */ -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; -} - -/** - * @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 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; -} - //======================================================================== // Helper Functions Implementation //======================================================================== @@ -503,22 +183,22 @@ Matrix numericalDifferential(std::function f, const Vecto * @return Lifted input in Lie Algebra * Uses Vector zero & Rot3 inverse, matrix functions */ -Vector lift(const State& xi, const Input& u) { - int n = xi.S.size(); - Vector L = Vector::Zero(6 + 3 * n); +template +Vector lift(const State& xi, const Input& u) { + Vector L = Vector::Zero(6 + 3 * N); - // First 3 elements - L.head<3>() = u.w - xi.b; + // First 3 elements + L.head<3>() = u.w - xi.b; - // Next 3 elements - L.segment<3>(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>(); - } + // 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; + return L; } /** * Implements group actions on the states @@ -533,19 +213,16 @@ Vector lift(const State& xi, const Input& u) { * @return Transformed state * Uses the Rot3 inverse and Vee functions */ -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"); - } +template +State operator*(const G& X, const State& xi) { + std::array new_S; - std::vector 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]); - } + for (size_t i = 0; i < N; i++) { + new_S[i] = 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); + return State(xi.R * X.A, X.A.inverse().matrix() * (xi.b - Rot3::Vee(X.a)), + new_S); } /** * Transforms the angular velocity measurements b/w frames @@ -555,8 +232,9 @@ State stateAction(const G& X, const State& xi) { * Uses Rot3 Inverse, matrix and Vee functions and is critical for maintaining * the input equivariance */ -Input velocityAction(const G& X, const Input& u) { - return Input{X.A.inverse().matrix() * (u.w - Rot3::Vee(X.a)), u.Sigma}; +template +Input velocityAction(const G& 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) @@ -566,64 +244,46 @@ Input velocityAction(const G& X, const Input& u) { * @return Transformed direction * Uses Rot3 inverse, matric and Unit3 unitvector functions */ -Vector3 outputAction(const G& X, const Unit3& y, int idx) { - if (idx == -1) { - return X.A.inverse().matrix() * y.unitVector(); - } else { - if (idx >= static_cast(X.B.size())) { - throw std::out_of_range("Calibration index out of range"); - } - return X.B[idx].inverse().matrix() * y.unitVector(); +template +Vector3 outputAction(const G& X, const Unit3& y, int idx) { + if (idx == -1) { + return X.A.inverse().matrix() * y.unitVector(); + } else { + if (idx >= static_cast(N)) { + throw std::out_of_range("Calibration index out of range"); } + return X.B[idx].inverse().matrix() * y.unitVector(); + } } /** - * Maps the error states to vector representations through exponential - * coordinates - * @param e error state - * @return Vector with local coordinates - * Uses Rot3 logamo for mapping rotations to the tangent space + * @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 */ -// 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"); -// } -// } -/** - * Used to convert the vectorized errors back to state space - * @param eps Local coordinates in the exponential parameterization - * @return State object corresponding to these coordinates - * Uses Rot3 expmap through the G::exp() function - */ -// State local_coords_inv(const Vector& eps) { -// G X = G::exp(eps); -// -// if (COORDINATE == "EXPONENTIAL") { -// std::vector 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 numericalDifferential(std::function 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 @@ -632,17 +292,17 @@ Vector3 outputAction(const G& X, const Unit3& y, int idx) { * @return A matrix representing the jacobian of the state action * Uses numericalDifferential, and Rot3 expmap, logmap */ -Matrix stateActionDiff(const State& xi) { - std::function coordsAction = - [&xi](const Vector& U) { - G groupElement = G::exp(U); - State transformed = stateAction(groupElement, xi); - return xi.localCoordinates(transformed); - }; +template +Matrix stateActionDiff(const State& xi) { + std::function coordsAction = [&xi](const Vector& U) { + G groupElement = G::exp(U); + State transformed = groupElement * xi; + return xi.localCoordinates(transformed); + }; - Vector zeros = Vector::Zero(6 + 3 * xi.S.size()); - Matrix differential = numericalDifferential(coordsAction, zeros); - return differential; + Vector zeros = Vector::Zero(6 + 3 * N); + Matrix differential = numericalDifferential(coordsAction, zeros); + return differential; } //======================================================================== @@ -656,38 +316,45 @@ Matrix stateActionDiff(const State& xi) { * @param m Number of sensors * Uses SelfAdjointSolver, completeOrthoganalDecomposition().pseudoInverse() */ -EqF::EqF(const Matrix& Sigma, int n, int m) - : dof(6 + 3 * n), n_cal(n), X_hat(G::identity(n)), - Sigma(Sigma), xi_0(State::identity(n)) { +template +EqF::EqF(const Matrix& Sigma, int m) + : dof(6 + 3 * N), + X_hat(G::identity(N)), + Sigma(Sigma), + xi_0(State::identity()) { + if (Sigma.rows() != dof || Sigma.cols() != dof) { + throw std::invalid_argument( + "Initial covariance dimensions must match the degrees of freedom"); + } - 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 eigensolver(Sigma); + if (eigensolver.eigenvalues().minCoeff() < -1e-10) { + throw std::invalid_argument( + "Covariance matrix must be semi-positive definite"); + } - // Check positive semi-definite - Eigen::SelfAdjointEigenSolver 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 (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"); + } - 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(); + // 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 */ -State EqF::stateEstimate() const { - return stateAction(X_hat, xi_0); +template +State EqF::stateEstimate() const { + return X_hat * xi_0; } /** * Implements the prediction step of the EqF using system dynamics and @@ -697,19 +364,19 @@ State EqF::stateEstimate() const { * @param dt time steps * Updated internal state and covariance */ -void EqF::propagation(const Input& u, double dt) { - State state_est = stateEstimate(); - Vector L = lift(state_est, u); +template +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 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; + Matrix tempSigma = blockDiag(u.Sigma, repBlock(1e-9 * I_3x3, N)); + 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; + X_hat = X_hat * G::exp(L * dt); + Sigma = Phi_DT * Sigma * Phi_DT.transpose() + M_DT; } /** * Implements the correction step of the filter using discrete measurements @@ -718,8 +385,9 @@ void EqF::propagation(const Input& u, double dt) { * * @param y Measurements */ -void EqF::update(const Measurement& y) { - if (y.cal_idx > n_cal) { +template +void EqF::update(const Measurement& y) { + if (y.cal_idx > static_cast(N)) { throw std::invalid_argument("Calibration index out of range"); } @@ -740,7 +408,7 @@ void EqF::update(const Measurement& y) { 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; + X_hat = G::exp(Delta) * X_hat; Sigma = (Matrix::Identity(dof, dof) - K * Ct) * Sigma; } /** @@ -749,20 +417,14 @@ void EqF::update(const Measurement& y) { * @return Linearized state matrix * Uses Matrix zero and Identity functions */ -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"); - } +template +Matrix EqF::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); } /** @@ -771,49 +433,35 @@ Matrix EqF::stateMatrixA(const Input& u) const { * @param dt time step * @return State transition matrix in discrete time */ -Matrix EqF::stateTransitionMatrix(const Input& u, double dt) const { - Matrix3 W0 = velocityAction(X_hat.inv(), u).W(); - Matrix Phi1 = Matrix::Zero(6, 6); +template +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; + Matrix3 Phi12 = -dt * (I_3x3 + (dt / 2) * W0 + ((dt * dt) / 6) * W0 * W0); + Matrix3 Phi22 = I_3x3 + 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"); - } + 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 */ -Matrix EqF::inputMatrixBt() const { - if (COORDINATE == "EXPONENTIAL") { - Matrix B1 = blockDiag(X_hat.A.matrix(), X_hat.A.matrix()); - Matrix B2; +template +Matrix EqF::inputMatrixBt() const { + Matrix B1 = blockDiag(X_hat.A.matrix(), X_hat.A.matrix()); + Matrix B2(3 * N, 3 * N); - for (const auto& B : X_hat.B) { - if (B2.size() == 0) { - B2 = B.matrix(); - } else { - B2 = blockDiag(B2, B.matrix()); - } - } + 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); - } else if (COORDINATE == "NORMAL") { - throw std::runtime_error("Normal coordinate representation is not implemented yet"); - } else { - throw std::invalid_argument("Invalid coordinate representation"); - } + return blockDiag(B1, B2); } /** * Computes the linearized measurement matrix. The structure depends on whether @@ -823,45 +471,49 @@ Matrix EqF::inputMatrixBt() const { * @return Measurement matrix * Uses the matrix zero, Rot3 hat and the Unitvector functions */ -Matrix EqF::measurementMatrixC(const Unit3& d, int idx) const { - Matrix Cc = Matrix::Zero(3, 3 * n_cal); +template +Matrix EqF::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()); - } + // 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()); + Matrix3 wedge_d = Rot3::Hat(d.unitVector()); - // Create the combined matrix - 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; + // 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; + return wedge_d * temp; } /** * Computes the measurement uncertainty propagation matrix * @param idx Calibration index * @return Returns B[idx] for calibrated sensors, A for uncalibrated */ -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(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(); +template +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(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 +struct traits> + : internal::LieGroupTraits> {}; +} // namespace gtsam - -} // namespace abc_eqf_lib - -#endif // ABC_EQF_H \ No newline at end of file +#endif // ABC_EQF_H \ No newline at end of file diff --git a/examples/ABC_EQF_Demo.cpp b/examples/ABC_EQF_Demo.cpp index 13b5a7da7..99b18d85f 100644 --- a/examples/ABC_EQF_Demo.cpp +++ b/examples/ABC_EQF_Demo.cpp @@ -3,17 +3,33 @@ * @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 + * 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; +constexpr size_t N = 1; // Number of calibration states +using M = abc_eqf_lib::State; +using Group = abc_eqf_lib::G; +using EqFilter = abc_eqf_lib::EqF; +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 y; /// Output measurements + int n_meas; /// Number of measurements + double t; /// Time + double dt; /// Time step +}; //======================================================================== // Data Processing Functions @@ -31,402 +47,398 @@ using namespace gtsam; * - 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 + * - 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 loadDataFromCSV(const std::string& filename, - int startRow = 0, - int maxRows = -1, - int downsample = 1); +std::vector 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_list, int printInterval = 10); +/// Process data with EqF and print summary results +void processDataWithEqF(EqFilter& filter, const std::vector& data_list, + int printInterval = 10); //======================================================================== // Data Processing Functions Implementation //======================================================================== -/** - * @brief Loads the test data from the csv file - * @param filename path to the csv file is specified - * @param startRow First row to load based on csv, 0 by default - * @param maxRows maximum rows to load, defaults to all rows - * @param downsample Downsample factor, default 1 - * @return A list of data objects -*/ +/* + * 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 loadDataFromCSV(const std::string& filename, int startRow, + int maxRows, int downsample) { + std::vector data_list; + std::ifstream file(filename); + if (!file.is_open()) { + throw std::runtime_error("Failed to open file: " + filename); + } -std::vector loadDataFromCSV(const std::string& filename, - int startRow, - int maxRows, - int downsample) { - std::vector data_list; - std::ifstream file(filename); + std::cout << "Loading data from " << filename << "..." << std::flush; - if (!file.is_open()) { - throw std::runtime_error("Failed to open file: " + filename); - } + std::string line; + int lineNumber = 0; + int rowCount = 0; + int errorCount = 0; + double prevTime = 0.0; - std::cout << "Loading data from " << filename << "..." << std::flush; + // Skip header + std::getline(file, line); + lineNumber++; - std::string line; - int lineNumber = 0; - int rowCount = 0; - int errorCount = 0; - double prevTime = 0.0; + // Skip to startRow + while (lineNumber < startRow && std::getline(file, line)) { + lineNumber++; + } - // Skip header - std::getline(file, line); + // Read data + while (std::getline(file, line) && (maxRows == -1 || rowCount < maxRows)) { lineNumber++; - // Skip to startRow - while (lineNumber < startRow && std::getline(file, line)) { - lineNumber++; + // Apply downsampling + if ((lineNumber - startRow - 1) % downsample != 0) { + continue; } - // Read data - while (std::getline(file, line) && (maxRows == -1 || rowCount < maxRows)) { - lineNumber++; + std::istringstream ss(line); + std::string token; + std::vector values; - // Apply downsampling - if ((lineNumber - startRow - 1) % downsample != 0) { - continue; - } - - std::istringstream ss(line); - std::string token; - std::vector 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::vector 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 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, 1, u, measurements, 2, t, dt}); - - rowCount++; - - // Show loading progress every 1000 rows - if (rowCount % 1000 == 0) { - std::cout << "." << std::flush; - } + // 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 + } } - std::cout << " Done!" << std::endl; - std::cout << "Loaded " << data_list.size() << " data points"; - - if (errorCount > 0) { - std::cout << " (" << errorCount << " errors encountered)"; + // Check if we have enough values + if (values.size() < 39) { + errorCount++; + continue; } - std::cout << std::endl; + // Extract values + double t = values[0]; + double dt = (rowCount == 0) ? 0.0 : t - prevTime; + prevTime = t; - return data_list; -} -/** - * @brief Takes in the data and runs an EqF on it and reports the results - * @param filter Initialized EqF filter - * @param data_list std::vector - * @param printInterval Progress indicator - * Prints the performance statstics like average error etc - * Uses Rot3 between, logmap and rpy functions - */ -void processDataWithEqF(EqF& filter, const std::vector& data_list, int printInterval) { - if (data_list.empty()) { - std::cerr << "No data to process" << std::endl; - return; + // 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 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 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 << "Processing " << data_list.size() << " data points with EqF..." << std::endl; + std::cout << " Done!" << std::endl; + std::cout << "Loaded " << data_list.size() << " data points"; - // Track performance metrics - std::vector att_errors; - std::vector bias_errors; - std::vector cal_errors; + if (errorCount > 0) { + std::cout << " (" << errorCount << " errors encountered)"; + } - // Track time for performance measurement - auto start = std::chrono::high_resolution_clock::now(); + std::cout << std::endl; - 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 - State 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 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(); - State 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; - } + return data_list; } -/** - * Main function for the EqF demo - * @param argc Number of arguments - * @param argv Array of arguments - * @return Exit code - */ +/// Takes in the data and runs an EqF on it and reports the results +void processDataWithEqF(EqFilter& filter, const std::vector& 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 att_errors; + std::vector bias_errors; + std::vector 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 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; + 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 + 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 = 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; + 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; + } } - std::cout << "\nEqF demonstration completed successfully." << std::endl; - return 0; + // 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 = + 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; } \ No newline at end of file