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 new file mode 100644 index 000000000..02b8dd4b9 --- /dev/null +++ b/examples/ABC_EQF.h @@ -0,0 +1,519 @@ +/** + * @file ABC_EQF.h + * @brief Header file for the Attitude-Bias-Calibration Equivariant Filter + * + * This file contains declarations for the Equivariant Filter (EqF) for attitude + * estimation with both gyroscope bias and sensor extrinsic calibration, based + * on the paper: "Overcoming Bias: Equivariant Filter Design for Biased Attitude + * Estimation with Online Calibration" by Fornasier et al. Authors: Darshan + * Rajasekaran & Jennifer Oum + */ + +#ifndef ABC_EQF_H +#define ABC_EQF_H +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include // For std::accumulate +#include +#include + +#include "ABC.h" + +// All implementations are wrapped in this namespace to avoid conflicts +namespace gtsam { +namespace abc_eqf_lib { + +using namespace std; +using namespace gtsam; + +//======================================================================== +// Helper Functions for EqF +//======================================================================== + +/// Calculate numerical differential + +Matrix numericalDifferential(std::function f, + const Vector& x); + +/** + * Compute the lift of the system (Theorem 3.8, Equation 7) + * @param xi State + * @param u Input + * @return Lift vector + */ +template +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 + */ +template +State operator*(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 + */ +template +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 + */ +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 + */ +template +Matrix stateActionDiff(const State& xi); + +//======================================================================== +// Equivariant Filter (EqF) +//======================================================================== + +/// Equivariant Filter (EqF) implementation +template +class EqF { + 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 transition matrix Phi (Equation 17) + * @param u Input + * @param dt Time step + * @return State transition matrix Phi + */ + Matrix stateTransitionMatrix(const Input& u, double dt) const; + + /** + * Return the Input matrix Bt + * @return Input matrix Bt + */ + Matrix inputMatrixBt() const; + + /** + * Return the measurement matrix C0 (Equation 14b) + * @param d Known direction + * @param idx Calibration index + * @return Measurement matrix C0 + */ + Matrix measurementMatrixC(const Unit3& d, int idx) const; + + /** + * Return the measurement output matrix Dt + * @param idx Calibration index + * @return Measurement output matrix Dt + */ + Matrix outputMatrixDt(int idx) const; + + public: + /** + * Initialize EqF + * @param Sigma Initial covariance + * @param m Number of sensors + */ + EqF(const Matrix& Sigma, int m); + + /** + * Return estimated state + * @return Current state estimate + */ + State stateEstimate() const; + + /** + * Propagate the filter state + * @param u Angular velocity measurement + * @param dt Time step + */ + void propagation(const Input& u, double dt); + + /** + * Update the filter state with a measurement + * @param y Direction measurement + */ + void update(const Measurement& y); +}; + +//======================================================================== +// Helper Functions Implementation +//======================================================================== + +/** + * Maps system dynamics to the symmetry group + * @param xi State + * @param u Input + * @return Lifted input in Lie Algebra + * Uses Vector zero & Rot3 inverse, matrix functions + */ +template +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; + + // Next 3 elements + L.segment<3>(3) = -u.W() * xi.b; + + // Remaining elements + for (size_t i = 0; i < N; i++) { + L.segment<3>(6 + 3 * i) = xi.S[i].inverse().matrix() * L.head<3>(); + } + + return L; +} +/** + * Implements group actions on the states + * @param X A symmetry group element G consisting of the attitude, bias and the + * calibration components X.a -> Rotation matrix containing the attitude X.b -> + * A skew-symmetric matrix representing bias X.B -> A vector of Rotation + * matrices for the calibration components + * @param xi State object + * xi.R -> Attitude (Rot3) + * xi.b -> Gyroscope Bias(Vector 3) + * xi.S -> Vector of calibration matrices(Rot3) + * @return Transformed state + * Uses the Rot3 inverse and Vee functions + */ +template +State operator*(const G& X, const State& xi) { + std::array new_S; + + 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); +} +/** + * Transforms the angular velocity measurements b/w frames + * @param X A symmetry group element X with the components + * @param u Inputs + * @return Transformed inputs + * Uses Rot3 Inverse, matrix and Vee functions and is critical for maintaining + * the input equivariance + */ +template +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) + * @param X Group element X + * @param y Direction measurement y + * @param idx Calibration index + * @return Transformed direction + * Uses Rot3 inverse, matric and Unit3 unitvector functions + */ +template +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(); + } +} + +/** + * @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; +} + +/** + * Computes the differential of a state action at the identity of the symmetry + * group + * @param xi State object Xi representing the point at which to evaluate the + * differential + * @return A matrix representing the jacobian of the state action + * Uses numericalDifferential, and Rot3 expmap, logmap + */ +template +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 * N); + Matrix differential = numericalDifferential(coordsAction, zeros); + return differential; +} + +//======================================================================== +// Equivariant Filter (EqF) Implementation +//======================================================================== +/** + * Initializes the EqF with state dimension validation and computes lifted + * innovation mapping + * @param Sigma Initial covariance + * @param n Number of calibration states + * @param m Number of sensors + * Uses SelfAdjointSolver, completeOrthoganalDecomposition().pseudoInverse() + */ +template +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"); + } + + // 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 (m <= 1) { + throw std::invalid_argument( + "Number of direction sensors must be at least 2"); + } + + // Compute differential of phi + Dphi0 = stateActionDiff(xi_0); + InnovationLift = Dphi0.completeOrthogonalDecomposition().pseudoInverse(); +} +/** + * Computes the internal group state to a physical state estimate + * @return Current state estimate + */ +template +State EqF::stateEstimate() const { + return X_hat * xi_0; +} +/** + * Implements the prediction step of the EqF using system dynamics and + * covariance propagation and advances the filter state by symmtery-preserving + * dynamics.Uses a Lie group integrator scheme for discrete time propagation + * @param u Angular velocity measurements + * @param dt time steps + * Updated internal state and covariance + */ +template +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 * 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; +} +/** + * Implements the correction step of the filter using discrete measurements + * Computes the measurement residual, Kalman gain and the updates both the state + * and covariance + * + * @param y Measurements + */ +template +void EqF::update(const Measurement& y) { + if (y.cal_idx > static_cast(N)) { + throw std::invalid_argument("Calibration index out of range"); + } + + // Get vector representations for checking + Vector3 y_vec = y.y.unitVector(); + Vector3 d_vec = y.d.unitVector(); + + // Skip update if any NaN values are present + if (std::isnan(y_vec[0]) || std::isnan(y_vec[1]) || std::isnan(y_vec[2]) || + std::isnan(d_vec[0]) || std::isnan(d_vec[1]) || std::isnan(d_vec[2])) { + return; // Skip this measurement + } + + Matrix Ct = measurementMatrixC(y.d, y.cal_idx); + Vector3 action_result = outputAction(X_hat.inv(), y.y, y.cal_idx); + Vector3 delta_vec = Rot3::Hat(y.d.unitVector()) * action_result; + Matrix Dt = outputMatrixDt(y.cal_idx); + Matrix S = Ct * Sigma * Ct.transpose() + Dt * y.Sigma * Dt.transpose(); + Matrix K = Sigma * Ct.transpose() * S.inverse(); + Vector Delta = InnovationLift * K * delta_vec; + X_hat = G::exp(Delta) * X_hat; + Sigma = (Matrix::Identity(dof, dof) - K * Ct) * Sigma; +} +/** + * Computes linearized continuous time state matrix + * @param u Angular velocity + * @return Linearized state matrix + * Uses Matrix zero and Identity functions + */ +template +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); +} + +/** + * Computes the discrete time state transition matrix + * @param u Angular velocity + * @param dt time step + * @return State transition matrix in discrete time + */ +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 * (I_3x3 + (dt / 2) * W0 + ((dt * dt) / 6) * W0 * W0); + Matrix3 Phi22 = I_3x3 + dt * W0 + ((dt * dt) / 2) * W0 * W0; + + Phi1.block<3, 3>(0, 0) = I_3x3; + Phi1.block<3, 3>(0, 3) = Phi12; + Phi1.block<3, 3>(3, 3) = Phi22; + Matrix Phi2 = repBlock(Phi22, N); + return blockDiag(Phi1, Phi2); +} +/** + * Computes the input uncertainty propagation matrix + * @return + * Uses the blockdiag matrix + */ +template +Matrix EqF::inputMatrixBt() const { + Matrix B1 = blockDiag(X_hat.A.matrix(), X_hat.A.matrix()); + Matrix B2(3 * N, 3 * N); + + for (size_t i = 0; i < N; ++i) { + B2.block<3, 3>(3 * i, 3 * i) = X_hat.B[i].matrix(); + } + + return blockDiag(B1, B2); +} +/** + * Computes the linearized measurement matrix. The structure depends on whether + * the sensor has a calibration state + * @param d reference direction + * @param idx Calibration index + * @return Measurement matrix + * Uses the matrix zero, Rot3 hat and the Unitvector functions + */ +template +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()); + } + + Matrix3 wedge_d = Rot3::Hat(d.unitVector()); + + // Create the combined matrix + Matrix temp(3, 6 + 3 * N); + temp.block<3, 3>(0, 0) = wedge_d; + temp.block<3, 3>(0, 3) = Matrix3::Zero(); + temp.block(0, 6, 3, 3 * N) = Cc; + + return wedge_d * temp; +} +/** + * Computes the measurement uncertainty propagation matrix + * @param idx Calibration index + * @return Returns B[idx] for calibrated sensors, A for uncalibrated + */ +template +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 + +#endif // ABC_EQF_H \ No newline at end of file diff --git a/examples/ABC_EQF_Demo.cpp b/examples/ABC_EQF_Demo.cpp new file mode 100644 index 000000000..99b18d85f --- /dev/null +++ b/examples/ABC_EQF_Demo.cpp @@ -0,0 +1,444 @@ +/** + * @file ABC_EQF_Demo.cpp + * @brief Demonstration of the full Attitude-Bias-Calibration Equivariant Filter + * + * This demo shows the Equivariant Filter (EqF) for attitude estimation + * with both gyroscope bias and sensor extrinsic calibration, based on the + * paper: "Overcoming Bias: Equivariant Filter Design for Biased Attitude + * Estimation with Online Calibration" by Fornasier et al. Authors: Darshan + * Rajasekaran & Jennifer Oum + */ + +#include "ABC_EQF.h" + +// Use namespace for convenience +using namespace gtsam; +constexpr size_t N = 1; // Number of calibration states +using M = abc_eqf_lib::State; +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 +//======================================================================== + +/** + * Load data from CSV file into a vector of Data objects for the EqF + * + * CSV format: + * - t: Time + * - q_w, q_x, q_y, q_z: True attitude quaternion + * - b_x, b_y, b_z: True bias + * - cq_w_0, cq_x_0, cq_y_0, cq_z_0: True calibration quaternion + * - w_x, w_y, w_z: Angular velocity measurements + * - std_w_x, std_w_y, std_w_z: Angular velocity measurement standard deviations + * - std_b_x, std_b_y, std_b_z: Bias process noise standard deviations + * - y_x_0, y_y_0, y_z_0, y_x_1, y_y_1, y_z_1: Direction measurements + * - std_y_x_0, std_y_y_0, std_y_z_0, std_y_x_1, std_y_y_1, std_y_z_1: Direction + * measurement standard deviations + * - d_x_0, d_y_0, d_z_0, d_x_1, d_y_1, d_z_1: Reference directions + * + */ +std::vector loadDataFromCSV(const std::string& filename, int startRow = 0, + int maxRows = -1, int downsample = 1); + +/// Process data with EqF and print summary results +void processDataWithEqF(EqFilter& filter, const std::vector& data_list, + int printInterval = 10); + +//======================================================================== +// Data Processing Functions Implementation +//======================================================================== + +/* + * Loads the test data from the csv file + * startRow First row to load based on csv, 0 by default + * maxRows maximum rows to load, defaults to all rows + * downsample Downsample factor, default 1 + * A list of data objects + */ + +std::vector 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::cout << "Loading data from " << filename << "..." << std::flush; + + std::string line; + int lineNumber = 0; + int rowCount = 0; + int errorCount = 0; + double prevTime = 0.0; + + // Skip header + std::getline(file, line); + lineNumber++; + + // Skip to startRow + while (lineNumber < startRow && std::getline(file, line)) { + lineNumber++; + } + + // Read data + while (std::getline(file, line) && (maxRows == -1 || rowCount < maxRows)) { + lineNumber++; + + // Apply downsampling + if ((lineNumber - startRow - 1) % downsample != 0) { + continue; + } + + std::istringstream ss(line); + std::string token; + std::vector values; + + // Parse line into values + while (std::getline(ss, token, ',')) { + try { + values.push_back(std::stod(token)); + } catch (const std::exception& e) { + errorCount++; + values.push_back(0.0); // Use default value + } + } + + // Check if we have enough values + if (values.size() < 39) { + errorCount++; + continue; + } + + // Extract values + double t = values[0]; + double dt = (rowCount == 0) ? 0.0 : t - prevTime; + prevTime = t; + + // Create ground truth state + Quaternion quat(values[1], values[2], values[3], values[4]); // w, x, y, z + Rot3 R = Rot3(quat); + + Vector3 b(values[5], values[6], values[7]); + + Quaternion calQuat(values[8], values[9], values[10], + values[11]); // w, x, y, z + std::array 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 << " Done!" << std::endl; + std::cout << "Loaded " << data_list.size() << " data points"; + + if (errorCount > 0) { + std::cout << " (" << errorCount << " errors encountered)"; + } + + std::cout << std::endl; + + return data_list; +} + +/// Takes in the data and runs an EqF on it and reports the results +void processDataWithEqF(EqFilter& filter, const std::vector& data_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; + + 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_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 diff --git a/gtsam/3rdparty/metis/CMakeLists.txt b/gtsam/3rdparty/metis/CMakeLists.txt index f8da7ce6f..8a577c73f 100644 --- a/gtsam/3rdparty/metis/CMakeLists.txt +++ b/gtsam/3rdparty/metis/CMakeLists.txt @@ -65,4 +65,4 @@ if (TARGET metis AND GKlib_COPTIONS) separate_arguments(GKlib_COPTIONS) # Declare those flags as to-be-imported in "client libraries", i.e. "gtsam" target_compile_options(metis INTERFACE ${GKlib_COPTIONS}) -endif() +endif() \ No newline at end of file