From cd1782e5d49b0ba83cd35c39425ecde535fa9552 Mon Sep 17 00:00:00 2001 From: darshan-17 Date: Tue, 15 Apr 2025 21:06:25 -0700 Subject: [PATCH] C++ implementation of the EqF to estimate bias. --- examples/ABC_EQF.cpp | 1009 +++++++++++++++++++++++++++++ examples/ABC_EQF.h | 427 ++++++++++++ examples/ABC_EQF/CMakeLists.txt | 14 - examples/ABC_EQF/Data.h | 41 -- examples/ABC_EQF/Direction.cpp | 13 - examples/ABC_EQF/Direction.h | 35 - examples/ABC_EQF/EqF.cpp | 285 -------- examples/ABC_EQF/EqF.h | 153 ----- examples/ABC_EQF/G.cpp | 61 -- examples/ABC_EQF/G.h | 63 -- examples/ABC_EQF/Input.cpp | 30 - examples/ABC_EQF/Input.h | 41 -- examples/ABC_EQF/Measurements.cpp | 17 - examples/ABC_EQF/Measurements.h | 36 - examples/ABC_EQF/State.cpp | 12 - examples/ABC_EQF/State.h | 29 - examples/ABC_EQF/main.cpp | 194 ------ examples/ABC_EQF/runEQF_withcsv.h | 683 ------------------- examples/ABC_EQF/utilities.cpp | 57 -- examples/ABC_EQF/utilities.h | 28 - examples/ABC_EQF_Demo.cpp | 89 +++ examples/CMakeLists.txt | 1 - gtsam/CMakeLists.txt | 5 +- 23 files changed, 1529 insertions(+), 1794 deletions(-) create mode 100644 examples/ABC_EQF.cpp create mode 100644 examples/ABC_EQF.h delete mode 100644 examples/ABC_EQF/CMakeLists.txt delete mode 100644 examples/ABC_EQF/Data.h delete mode 100644 examples/ABC_EQF/Direction.cpp delete mode 100644 examples/ABC_EQF/Direction.h delete mode 100644 examples/ABC_EQF/EqF.cpp delete mode 100644 examples/ABC_EQF/EqF.h delete mode 100644 examples/ABC_EQF/G.cpp delete mode 100644 examples/ABC_EQF/G.h delete mode 100644 examples/ABC_EQF/Input.cpp delete mode 100644 examples/ABC_EQF/Input.h delete mode 100644 examples/ABC_EQF/Measurements.cpp delete mode 100644 examples/ABC_EQF/Measurements.h delete mode 100644 examples/ABC_EQF/State.cpp delete mode 100644 examples/ABC_EQF/State.h delete mode 100644 examples/ABC_EQF/main.cpp delete mode 100644 examples/ABC_EQF/runEQF_withcsv.h delete mode 100644 examples/ABC_EQF/utilities.cpp delete mode 100644 examples/ABC_EQF/utilities.h create mode 100644 examples/ABC_EQF_Demo.cpp diff --git a/examples/ABC_EQF.cpp b/examples/ABC_EQF.cpp new file mode 100644 index 000000000..8209e7b29 --- /dev/null +++ b/examples/ABC_EQF.cpp @@ -0,0 +1,1009 @@ +/** + * @file ABC_EQF.cpp + * @brief Implementation of the Attitude-Bias-Calibration Equivariant Filter + * + * This file contains the implementation 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 + */ + +#include "ABC_EQF.h" + +namespace abc_eqf_lib { + +// 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; +} + +//======================================================================== +// Direction Class Implementation +//======================================================================== + +/** + * @brief Initializes a direction object vector from a provided 3D vector input + * @param d_vec A 3-D vector that should have a unit norm(This represents a + * direction in 3D space) Uses Unit3's constructor which normalizes vectors + */ + + +Direction::Direction(const Vector3& d_vec) : d(d_vec) { + if (!checkNorm(d_vec)) { + throw std::invalid_argument("Direction must be a unit vector"); + } +} + /** Access the individual components of the direction vector defined above using this methods below + * Uses the Unit3 object from GTSAM to compute the components + */ + +double Direction::x() const { + return d.unitVector()[0]; +} + +double Direction::y() const { + return d.unitVector()[1]; +} + +double Direction::z() const { + return d.unitVector()[2]; +} + +bool Direction::hasNaN() const { + Vector3 vec = d.unitVector(); + return std::isnan(vec[0]) || std::isnan(vec[1]) || std::isnan(vec[2]); +} + +//======================================================================== +// Input Class Implementation +//======================================================================== +/** + * @brief Constructs an input object using the Angular velocity vector and the + * covariance matrix + * @param w Angular vector + * @param Sigma 6X6 covariance matrix + * Uses Matrix's rows(), cols() and Eigen's SelfAdjointEigenSolver + */ +Input::Input(const Vector3& w, const Matrix& Sigma) : w(w), Sigma(Sigma) { + if (Sigma.rows() != 6 || Sigma.cols() != 6) { + throw std::invalid_argument("Input measurement noise covariance must be 6x6"); + } + + // Check positive semi-definite + Eigen::SelfAdjointEigenSolver eigensolver(Sigma); + if (eigensolver.eigenvalues().minCoeff() < -1e-10) { + throw std::invalid_argument("Covariance matrix must be semi-positive definite"); + } +} +/** + * + * @return 3X3 skey symmetric matrix when called + * Uses Rot3's Hat() to create skew-symmetric matrix + */ +Matrix3 Input::W() const { + return Rot3::Hat(w); +} + + +//======================================================================== +// Measurement Class Implementation +//======================================================================== +/** + * @brief Constructs measurement with directions and covariance. + * @param y_vec A 3D vector representing the measured direction in the sensor frame + * @param d_vec A 3D vector representing the known reference direction in the global frame aka ground truth direction + * @param Sigma 3x3 positive definite covariance vector representing the uncertainty in the measurements + * @param i Calibration index - A non-negative integer specifies the element in the calibration vector + * that corresponds to the sensor of interest. A value of -1 indicates that all the sensors have been calibrated + * + * Creates a measurement object that stores the measured direction(y), reference direction(d), measurement noise covariance(Sigma) + * and Calibration Index cal_idx + * + * Uses Eigen's SelfAdjointEigenSolver + * + */ + +Measurement::Measurement(const Vector3& y_vec, const Vector3& d_vec, + const Matrix3& Sigma, int i) + : y(y_vec), d(d_vec), Sigma(Sigma), cal_idx(i) { + + // Check positive semi-definite + Eigen::SelfAdjointEigenSolver eigensolver(Sigma); + if (eigensolver.eigenvalues().minCoeff() < -1e-10) { + throw std::invalid_argument("Covariance matrix must be semi-positive definite"); + } +} + +//======================================================================== +// State Class Implementation +//======================================================================== +/** + * + * @param R Rot3 (Attitude) + * @param b Vector (Bias) + * @param S Vector (Rot 3 calibration states) + * Combines the navigation and the calibration states together and provides a + * mechanism to represent the complete system + * + */ +State::State(const Rot3& R, const Vector3& b, const std::vector& S) + : R(R), b(b), S(S) {} + +/** + * + * @param n Number of Calibration states + * @return State object intitialized to identity + * Creates a default/ initial state + * Uses GTSAM's Rot3::identity and Vector3 zero function + */ +State State::identity(int n) { + std::vector calibrations(n, Rot3::Identity()); + return State(Rot3::Identity(), Vector3::Zero(), calibrations); +} + +//======================================================================== +// Data Struct Implementation +//======================================================================== + +/** + * + * @param xi Ground Truth state + * @param n_cal Number of calibration states + * @param u Input measurements + * @param y Vector of y measurements + * @param n_meas number of measurements + * @param t timestamp + * @param dt time step + * Used to organize the state, input and measurement data with timestamps for + * testing Uses Rot3, Vector 3 and Unit3 classes + */ +Data::Data(const State& xi, int n_cal, const Input& u, + const std::vector& y, int n_meas, + double t, double dt) + : xi(xi), n_cal(n_cal), u(u), y(y), n_meas(n_meas), t(t), dt(dt) {} + +//======================================================================== +// Symmetry Group Implementation - Group Elements and actions +//======================================================================== +/** + * + * @param A Attitude element of Rot3 type + * @param a Matrix3 bias element + * @param B Rot3 vector containing calibration elements + * Ouptuts a G object using Rot3 for rotation representation + */ +G::G(const Rot3& A, const Matrix3& a, const std::vector& B) + : A(A), a(a), B(B) {} + +/** + * Defines the group operation (multiplication) + * @param other Another Group element + * @return G a product of two group elements + * Uses Rot3 Hat, Rot3 Vee for multiplication + * + */ +G G::operator*(const G& other) const { + if (B.size() != other.B.size()) { + throw std::invalid_argument("Group elements must have the same number of calibration elements"); + } + + std::vector 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); +} + +/** + * Used to compute the Group inverse + * @return The inverse of group element + * Uses Rot3 inverse, Rot3 matrix, hat and vee functions + * The left invariant property of the semi-direct product group structure is implemented here by using the -ve sign + */ +G 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); +} + +/** + * Creates the identity element of the group + * @param n Number of calibration elements + * @return the identity element + * Uses Rot3 Identity and Matrix zero + */ +G G::identity(int n) { + std::vector B(n, Rot3::Identity()); + return G(Rot3::Identity(), Matrix3::Zero(), B); +} +/** + * Maps the tangent space elements to the group + * @param x Vector in lie algebra + * @return the group element G + * Uses Rot3 expmap and Expmapderivative function + */ +G G::exp(const Vector& x) { + if (x.size() < 6 || x.size() % 3 != 0) { + throw std::invalid_argument("Wrong size, a vector with size multiple of 3 and at least 6 must be provided"); + } + + int n = (x.size() - 6) / 3; + Rot3 A = Rot3::Expmap(x.head<3>()); + + Vector3 a_vee = Rot3::ExpmapDerivative(-x.head<3>()) * x.segment<3>(3); + Matrix3 a = Rot3::Hat(a_vee); + + std::vector 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 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 + */ +Vector lift(const State& xi, const Input& u) { + int n = xi.S.size(); + Vector L = Vector::Zero(6 + 3 * n); + + // First 3 elements + L.head<3>() = u.w - xi.b; + + // Next 3 elements + L.segment<3>(3) = -u.W() * xi.b; + + // Remaining elements + for (int i = 0; i < n; i++) { + L.segment<3>(6 + 3*i) = xi.S[i].inverse().matrix() * L.head<3>(); + } + + return L; +} +/** + * 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 + */ +State stateAction(const G& X, const State& xi) { + if (xi.S.size() != X.B.size()) { + throw std::invalid_argument("Number of calibration states and B elements must match"); + } + + std::vector new_S; + for (size_t i = 0; i < X.B.size(); i++) { + new_S.push_back(X.A.inverse() * xi.S[i] * X.B[i]); + } + + return State(xi.R * X.A, + X.A.inverse().matrix() * (xi.b - Rot3::Vee(X.a)), + new_S); +} +/** + * 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 + */ +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 + */ +Vector3 outputAction(const G& X, const Direction& y, int idx) { + if (idx == -1) { + return X.A.inverse().matrix() * y.d.unitVector(); + } else { + if (idx >= static_cast(X.B.size())) { + throw std::out_of_range("Calibration index out of range"); + } + return X.B[idx].inverse().matrix() * y.d.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 + */ +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"); + } +} +/** + * 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 + */ +Matrix stateActionDiff(const State& xi) { + std::function coordsAction = + [&xi](const Vector& U) { + return local_coords(stateAction(G::exp(U), xi)); + }; + + Vector zeros = Vector::Zero(6 + 3 * xi.S.size()); + Matrix differential = numericalDifferential(coordsAction, zeros); + return differential; +} + +//======================================================================== +// 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() + */ +EqF::EqF(const Matrix& Sigma, int n, int m) + : __dof(6 + 3 * n), __n_cal(n), __n_sensor(m), __X_hat(G::identity(n)), + __Sigma(Sigma), __xi_0(State::identity(n)) { + + if (Sigma.rows() != __dof || Sigma.cols() != __dof) { + throw std::invalid_argument("Initial covariance dimensions must match the degrees of freedom"); + } + + // Check positive semi-definite + Eigen::SelfAdjointEigenSolver 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 + */ +State EqF::stateEstimate() const { + return stateAction(__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 + */ +void EqF::propagation(const Input& u, double dt) { + State state_est = stateEstimate(); + Vector L = lift(state_est, u); + + Matrix Phi_DT = __stateTransitionMatrix(u, dt); + Matrix Bt = __inputMatrixBt(); + + Matrix tempSigma = blockDiag(u.Sigma, + repBlock(1e-9 * Matrix3::Identity(), __n_cal)); + Matrix M_DT = (Bt * tempSigma * Bt.transpose()) * dt; + + __X_hat = __X_hat * G::exp(L * dt); + __Sigma = Phi_DT * __Sigma * Phi_DT.transpose() + M_DT; +} +/** + * 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 + */ +void EqF::update(const Measurement& y) { + if (y.cal_idx > __n_cal) { + throw std::invalid_argument("Calibration index out of range"); + } + + // Get vector representations for checking + Vector3 y_vec = y.y.d.unitVector(); + Vector3 d_vec = y.d.d.unitVector(); + + // Skip update if any NaN values are present + if (std::isnan(y_vec[0]) || std::isnan(y_vec[1]) || std::isnan(y_vec[2]) || + std::isnan(d_vec[0]) || std::isnan(d_vec[1]) || std::isnan(d_vec[2])) { + return; // Skip this measurement + } + + Matrix Ct = __measurementMatrixC(y.d, y.cal_idx); + Vector3 action_result = outputAction(__X_hat.inv(), y.y, y.cal_idx); + Vector3 delta_vec = Rot3::Hat(y.d.d.unitVector()) * action_result; + 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 + */ +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"); + } +} + +/** + * Computes the discrete time state transition matrix + * @param u Angular velocity + * @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); + + Matrix3 Phi12 = -dt * (Matrix3::Identity() + (dt / 2) * W0 + ((dt*dt) / 6) * W0 * W0); + Matrix3 Phi22 = Matrix3::Identity() + dt * W0 + ((dt*dt) / 2) * W0 * W0; + + if (COORDINATE == "EXPONENTIAL") { + Phi1.block<3, 3>(0, 0) = Matrix3::Identity(); + Phi1.block<3, 3>(0, 3) = Phi12; + Phi1.block<3, 3>(3, 3) = Phi22; + Matrix Phi2 = repBlock(Phi22, __n_cal); + return blockDiag(Phi1, Phi2); + } else if (COORDINATE == "NORMAL") { + throw std::runtime_error("Normal coordinate representation is not implemented yet"); + } else { + throw std::invalid_argument("Invalid coordinate representation"); + } +} +/** + * 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; + + for (const auto& B : __X_hat.B) { + if (B2.size() == 0) { + B2 = B.matrix(); + } else { + B2 = blockDiag(B2, B.matrix()); + } + } + + return blockDiag(B1, B2); + } else if (COORDINATE == "NORMAL") { + throw std::runtime_error("Normal coordinate representation is not implemented yet"); + } else { + throw std::invalid_argument("Invalid coordinate representation"); + } +} +/** + * 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 + */ +Matrix EqF::__measurementMatrixC(const Direction& d, int idx) const { + Matrix Cc = Matrix::Zero(3, 3 * __n_cal); + + // If the measurement is related to a sensor that has a calibration state + if (idx >= 0) { + // Set the correct 3x3 block in Cc + Cc.block<3, 3>(0, 3 * idx) = Rot3::Hat(d.d.unitVector()); + } + + Matrix3 wedge_d = Rot3::Hat(d.d.unitVector()); + + // 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; + + 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(); + } +} + +//======================================================================== +// 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 +*/ + + + +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::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(y0, d0, covY0, 0)); + + // Second measurement (calibrated sensor, cal_idx = -1) + Vector3 y1(values[24], values[25], values[26]); + Vector3 d1(values[36], values[37], values[38]); + + // Normalize vectors if needed + if (abs(y1.norm() - 1.0) > 1e-5) y1.normalize(); + if (abs(d1.norm() - 1.0) > 1e-5) d1.normalize(); + + // Measurement covariance + Matrix3 covY1 = Matrix3::Zero(); + covY1(0, 0) = values[30] * values[30]; // std_y_x_1^2 + covY1(1, 1) = values[31] * values[31]; // std_y_y_1^2 + covY1(2, 2) = values[32] * values[32]; // std_y_z_1^2 + + // Create measurement + measurements.push_back(Measurement(y1, d1, covY1, -1)); + + // Create Data object and add to list + data_list.push_back(Data(xi, 1, u, measurements, 2, t, dt)); + + rowCount++; + + // 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; +} +/** + * @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; + } + + 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 + if (y.y.hasNaN() || y.d.hasNaN()) { + 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; + } +} + +} // namespace abc_eqf_lib \ No newline at end of file diff --git a/examples/ABC_EQF.h b/examples/ABC_EQF.h new file mode 100644 index 000000000..6d381b1ee --- /dev/null +++ b/examples/ABC_EQF.h @@ -0,0 +1,427 @@ +/** + * @file ABC_EQF.h + * @brief Header file for the Attitude-Bias-Calibration Equivariant Filter + * + * This file contains declarations for the Equivariant Filter (EqF) for attitude estimation + * with both gyroscope bias and sensor extrinsic calibration, based on the paper: + * "Overcoming Bias: Equivariant Filter Design for Biased Attitude Estimation + * with Online Calibration" by Fornasier et al. + * Authors: Darshan Rajasekaran & Jennifer Oum + */ + + +#ifndef ABC_EQF_H +#define ABC_EQF_H +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include // For std::accumulate + +// All implementations are wrapped in this namespace to avoid conflicts +namespace abc_eqf_lib { + +using namespace std; +using namespace gtsam; + +// Global configuration +// Define coordinate type: "EXPONENTIAL" or "NORMAL" +extern const std::string COORDINATE; + +//======================================================================== +// Utility Functions +//======================================================================== + +/** + * Check if a vector is a unit vector + */ +bool checkNorm(const Vector3& x, double tol = 1e-3); + +/** + * Check if vector contains NaN values + */ +bool hasNaN(const Vector3& vec); + +/** + * Create a block diagonal matrix from two matrices + */ +Matrix blockDiag(const Matrix& A, const Matrix& B); + +/** + * Repeat a block matrix n times along the diagonal + */ +Matrix repBlock(const Matrix& A, int n); + +/** + * Calculate numerical differential + */ +Matrix numericalDifferential(std::function f, const Vector& x); + +//======================================================================== +// Core Data Types +//======================================================================== + +/** + * Direction class as a S2 element + */ +class Direction { +public: + Unit3 d; // Direction (unit vector on S2) + + /** + * Initialize direction + * @param d_vec Direction vector (must be unit norm) + */ + Direction(const Vector3& d_vec); + + // Accessor methods for vector components + double x() const; + double y() const; + double z() const; + + // Check if the direction contains NaN values + bool hasNaN() const; +}; + +/** + * Input class for the Biased Attitude System + */ +class Input { +public: + Vector3 w; // Angular velocity + Matrix Sigma; // Noise covariance + + /** + * Initialize Input + * @param w Angular velocity (3-vector) + * @param Sigma Noise covariance (6x6 matrix) + */ + Input(const Vector3& w, const Matrix& Sigma); + + /** + * Return the Input as a skew-symmetric matrix + * @return w as a skew-symmetric matrix + */ + Matrix3 W() const; + + /** + * Return a random angular velocity + * @return A random angular velocity as Input element + */ + static Input random(); +}; + +/** + * Measurement class + * cal_idx is an index corresponding to the calibration related to the measurement + * cal_idx = -1 indicates the measurement is from a calibrated sensor + */ +class Measurement { +public: + Direction y; // Measurement direction in sensor frame + Direction d; // Known direction in global frame + Matrix3 Sigma; // Covariance matrix of the measurement + int cal_idx = -1; // Calibration index (-1 for calibrated sensor) + + /** + * Initialize measurement + * @param y_vec Direction measurement in sensor frame + * @param d_vec Known direction in global frame + * @param Sigma Measurement noise covariance + * @param i Calibration index (-1 for calibrated sensor) + */ + Measurement(const Vector3& y_vec, const Vector3& d_vec, + const Matrix3& Sigma, int i = -1); +}; + +/** + * State class representing the state of the Biased Attitude System + */ +class State { +public: + Rot3 R; // Attitude rotation matrix R + Vector3 b; // Gyroscope bias b + std::vector S; // Sensor calibrations S + + State(const Rot3& R = Rot3::Identity(), + const Vector3& b = Vector3::Zero(), + const std::vector& S = std::vector()); + + static State identity(int n); +}; + +/** + * Data structure for ground-truth, input and output data + */ +struct Data { + State xi; // Ground-truth state + int n_cal; // Number of calibration states + Input u; // Input measurements + std::vector y; // Output measurements + int n_meas; // Number of measurements + double t; // Time + double dt; // Time step + + /** + * Initialize Data + * @param xi Ground-truth state + * @param n_cal Number of calibration states + * @param u Input measurements + * @param y Output measurements + * @param n_meas Number of measurements + * @param t Time + * @param dt Time step + */ + Data(const State& xi, int n_cal, const Input& u, + const std::vector& y, int n_meas, + double t, double dt); +}; + +//======================================================================== +// Symmetry Group +//======================================================================== + +/** + * Symmetry group (SO(3) |x so(3)) x SO(3) x ... x SO(3) + * Each element of the B list is associated with a calibration state + */ +class G { +public: + Rot3 A; // First SO(3) element + Matrix3 a; // so(3) element (skew-symmetric matrix) + std::vector B; // List of SO(3) elements for calibration + + /** + * Initialize the symmetry group G + * @param A SO3 element + * @param a so(3) element (skew symmetric matrix) + * @param B list of SO3 elements + */ + G(const Rot3& A = Rot3::Identity(), + const Matrix3& a = Matrix3::Zero(), + const std::vector& B = std::vector()); + + /** + * Define the group operation (multiplication) + * @param other Another group element + * @return The product of this and other + */ + G operator*(const G& other) const; + + /** + * Return the inverse element of the symmetry group + * @return The inverse of this group element + */ + G inv() const; + + /** + * Return the identity of the symmetry group + * @param n Number of calibration elements + * @return The identity element with n calibration components + */ + static G identity(int n); + + /** + * Return a group element X given by X = exp(x) + * @param x Vector representation of Lie algebra element + * @return Group element given by the exponential of x + */ + static G exp(const Vector& x); +}; + +//======================================================================== +// Helper Functions for EqF +//======================================================================== + +/** + * Compute the lift of the system (Theorem 3.8, Equation 7) + * @param xi State + * @param u Input + * @return Lift vector + */ +Vector lift(const State& xi, const Input& u); + +/** + * Action of the symmetry group on the state space (Equation 4) + * @param X Group element + * @param xi State + * @return New state after group action + */ +State stateAction(const G& X, const State& xi); + +/** + * Action of the symmetry group on the input space (Equation 5) + * @param X Group element + * @param u Input + * @return New input after group action + */ +Input velocityAction(const G& X, const Input& u); + +/** + * Action of the symmetry group on the output space (Equation 6) + * @param X Group element + * @param y Direction measurement + * @param idx Calibration index + * @return New direction after group action + */ +Vector3 outputAction(const G& X, const Direction& y, int idx); + +/** + * Local coordinates assuming xi_0 = identity (Equation 9) + * @param e State representing equivariant error + * @return Local coordinates + */ +Vector local_coords(const State& e); + +/** + * Local coordinates inverse assuming xi_0 = identity + * @param eps Local coordinates + * @return Corresponding state + */ +State local_coords_inv(const Vector& eps); + +/** + * Differential of the phi action at E = Id in local coordinates + * @param xi State + * @return Differential matrix + */ +Matrix stateActionDiff(const State& xi); + +//======================================================================== +// Equivariant Filter (EqF) +//======================================================================== + +/** + * Equivariant Filter (EqF) implementation + */ +class EqF { +private: + int __dof; // Degrees of freedom + int __n_cal; // Number of calibration states + int __n_sensor; // Number of sensors + G __X_hat; // Filter state + Matrix __Sigma; // Error covariance + State __xi_0; // Origin state + Matrix __Dphi0; // Differential of phi at origin + Matrix __InnovationLift; // Innovation lift matrix + + /** + * Return the state matrix A0t (Equation 14a) + * @param u Input + * @return State matrix A0t + */ + Matrix __stateMatrixA(const Input& u) const; + + /** + * Return the state transition matrix Phi (Equation 17) + * @param u Input + * @param dt Time step + * @return State transition matrix Phi + */ + Matrix __stateTransitionMatrix(const Input& u, double dt) const; + + /** + * Return the Input matrix Bt + * @return Input matrix Bt + */ + Matrix __inputMatrixBt() const; + + /** + * Return the measurement matrix C0 (Equation 14b) + * @param d Known direction + * @param idx Calibration index + * @return Measurement matrix C0 + */ + Matrix __measurementMatrixC(const Direction& d, int idx) const; + + /** + * Return the measurement output matrix Dt + * @param idx Calibration index + * @return Measurement output matrix Dt + */ + Matrix __outputMatrixDt(int idx) const; + +public: + /** + * Initialize EqF + * @param Sigma Initial covariance + * @param n Number of calibration states + * @param m Number of sensors + */ + EqF(const Matrix& Sigma, int n, int m); + + /** + * Return estimated state + * @return Current state estimate + */ + State stateEstimate() const; + + /** + * Propagate the filter state + * @param u Angular velocity measurement + * @param dt Time step + */ + void propagation(const Input& u, double dt); + + /** + * Update the filter state with a measurement + * @param y Direction measurement + */ + void update(const Measurement& y); +}; + +//======================================================================== +// Data Processing Functions +//======================================================================== + +/** + * Load data from CSV file into a vector of Data objects for the EqF + * + * CSV format: + * - t: Time + * - q_w, q_x, q_y, q_z: True attitude quaternion + * - b_x, b_y, b_z: True bias + * - cq_w_0, cq_x_0, cq_y_0, cq_z_0: True calibration quaternion + * - w_x, w_y, w_z: Angular velocity measurements + * - std_w_x, std_w_y, std_w_z: Angular velocity measurement standard deviations + * - std_b_x, std_b_y, std_b_z: Bias process noise standard deviations + * - y_x_0, y_y_0, y_z_0, y_x_1, y_y_1, y_z_1: Direction measurements + * - std_y_x_0, std_y_y_0, std_y_z_0, std_y_x_1, std_y_y_1, std_y_z_1: Direction measurement standard deviations + * - d_x_0, d_y_0, d_z_0, d_x_1, d_y_1, d_z_1: Reference directions + * + * @param filename Path to the CSV file + * @param startRow First row to load (default: 0) + * @param maxRows Maximum number of rows to load (default: all) + * @param downsample Downsample factor (default: 1, which means no downsampling) + * @return Vector of Data objects + */ +std::vector 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); + +} // namespace abc_eqf_lib + +#endif // ABC_EQF_H \ No newline at end of file diff --git a/examples/ABC_EQF/CMakeLists.txt b/examples/ABC_EQF/CMakeLists.txt deleted file mode 100644 index 23eb309d2..000000000 --- a/examples/ABC_EQF/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -add_executable(ABC_EqF - main.cpp - EqF.cpp - State.cpp - Input.cpp - G.cpp - Direction.cpp - Measurements.cpp - utilities.cpp - runEQF_withcsv.h -) - -target_link_libraries(ABC_EqF gtsam) - diff --git a/examples/ABC_EQF/Data.h b/examples/ABC_EQF/Data.h deleted file mode 100644 index 2e6005572..000000000 --- a/examples/ABC_EQF/Data.h +++ /dev/null @@ -1,41 +0,0 @@ -// -// Created by darshan on 3/11/25. -// - -#ifndef DATA_H -#define DATA_H -//#pragma once - -#include "State.h" -#include "Input.h" -#include "Measurements.h" -#include - -/** - * 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 - - /** - * Initialize Data - * @param xi Ground-truth state - * @param n_cal Number of calibration states - * @param u Input measurements - * @param y Output measurements - * @param n_meas Number of measurements - * @param t Time - * @param dt Time step - */ - Data(const State& xi, int n_cal, const Input& u, - const std::vector& y, int n_meas, - double t, double dt) - : xi(xi), n_cal(n_cal), u(u), y(y), n_meas(n_meas), t(t), dt(dt) {} -}; -#endif //DATA_H diff --git a/examples/ABC_EQF/Direction.cpp b/examples/ABC_EQF/Direction.cpp deleted file mode 100644 index 751324b0b..000000000 --- a/examples/ABC_EQF/Direction.cpp +++ /dev/null @@ -1,13 +0,0 @@ -// -// Created by darshan on 3/11/25. -// - -#include "Direction.h" -#include "utilities.h" -#include - -Direction::Direction(const Vector3& d_vec) : d(d_vec) { - if (!checkNorm(d_vec)) { - throw std::invalid_argument("Direction must be a unit vector"); - } -} \ No newline at end of file diff --git a/examples/ABC_EQF/Direction.h b/examples/ABC_EQF/Direction.h deleted file mode 100644 index 041b90bb6..000000000 --- a/examples/ABC_EQF/Direction.h +++ /dev/null @@ -1,35 +0,0 @@ -// -// Created by darshan on 3/11/25. -// - -#ifndef DIRECTION_H -#define DIRECTION_H -//#pragma once -#include -#include -using namespace gtsam; -/** - * Direction class as a S2 element - */ -class Direction { -public: - Unit3 d; // Direction (unit vector on S2) - - /** - * Initialize direction - * @param d_vec Direction vector (must be unit norm) - */ - Direction(const Vector3& d_vec); - - // Accessor methods for vector components - double x() const { return d.unitVector()[0]; } - double y() const { return d.unitVector()[1]; } - double z() const { return d.unitVector()[2]; } - - // Check if the direction contains NaN values - bool hasNaN() const { - Vector3 vec = d.unitVector(); - return std::isnan(vec[0]) || std::isnan(vec[1]) || std::isnan(vec[2]); - } -}; -#endif //DIRECTION_H diff --git a/examples/ABC_EQF/EqF.cpp b/examples/ABC_EQF/EqF.cpp deleted file mode 100644 index 32414bb74..000000000 --- a/examples/ABC_EQF/EqF.cpp +++ /dev/null @@ -1,285 +0,0 @@ -// -// Created by darshan on 3/11/25. -// -#include "EqF.h" -#include "utilities.h" -#include -#include -#include - -// Implementation of helper functions - -Vector lift(const State& xi, const Input& u) { - int n = xi.S.size(); - Vector L = Vector::Zero(6 + 3 * n); - - // First 3 elements - L.head<3>() = u.w - xi.b; - - // Next 3 elements - L.segment<3>(3) = -u.W() * xi.b; - - // Remaining elements - for (int i = 0; i < n; i++) { - L.segment<3>(6 + 3*i) = xi.S[i].inverse().matrix() * L.head<3>(); - } - - return L; -} - -State stateAction(const G& X, const State& xi) { - if (xi.S.size() != X.B.size()) { - throw std::invalid_argument("Number of calibration states and B elements must match"); - } - - std::vector new_S; - for (size_t i = 0; i < X.B.size(); i++) { - new_S.push_back(X.A.inverse() * xi.S[i] * X.B[i]); - } - - return State(xi.R * X.A, - X.A.inverse().matrix() * (xi.b - Rot3::Vee(X.a)), - new_S); -} - -Input velocityAction(const G& X, const Input& u) { - return Input(X.A.inverse().matrix() * (u.w - Rot3::Vee(X.a)), u.Sigma); -} - -Vector3 outputAction(const G& X, const Direction& y, int idx) { - if (idx == -1) { - return X.A.inverse().matrix() * y.d.unitVector(); - } else { - if (idx >= static_cast(X.B.size())) { - throw std::out_of_range("Calibration index out of range"); - } - return X.B[idx].inverse().matrix() * y.d.unitVector(); - } -} - -Vector local_coords(const State& e) { - if (COORDINATE == "EXPONENTIAL") { - Vector eps(6 + 3 * e.S.size()); - - // First 3 elements - eps.head<3>() = Rot3::Logmap(e.R); - - // Next 3 elements - eps.segment<3>(3) = e.b; - - // Remaining elements - for (size_t i = 0; i < e.S.size(); i++) { - eps.segment<3>(6 + 3*i) = Rot3::Logmap(e.S[i]); - } - - return eps; - } else if (COORDINATE == "NORMAL") { - throw std::runtime_error("Normal coordinate representation is not implemented yet"); - } else { - throw std::invalid_argument("Invalid coordinate representation"); - } -} - -State local_coords_inv(const Vector& eps) { - G X = G::exp(eps); - - if (COORDINATE == "EXPONENTIAL") { - std::vector S = X.B; - return State(X.A, eps.segment<3>(3), S); - } else if (COORDINATE == "NORMAL") { - throw std::runtime_error("Normal coordinate representation is not implemented yet"); - } else { - throw std::invalid_argument("Invalid coordinate representation"); - } -} - -Matrix stateActionDiff(const State& xi) { - std::function coordsAction = - [&xi](const Vector& U) { - return local_coords(stateAction(G::exp(U), xi)); - }; - - Vector zeros = Vector::Zero(6 + 3 * xi.S.size()); - Matrix differential = numericalDifferential(coordsAction, zeros); - return differential; -} - -// EqF class implementation - -EqF::EqF(const Matrix& Sigma, int n, int m) - : __dof(6 + 3 * n), __n_cal(n), __n_sensor(m), __X_hat(G::identity(n)), - __Sigma(Sigma), __xi_0(State::identity(n)) { - - if (Sigma.rows() != __dof || Sigma.cols() != __dof) { - throw std::invalid_argument("Initial covariance dimensions must match the degrees of freedom"); - } - - // Check positive semi-definite - Eigen::SelfAdjointEigenSolver eigensolver(Sigma); - if (eigensolver.eigenvalues().minCoeff() < -1e-10) { - throw std::invalid_argument("Covariance matrix must be semi-positive definite"); - } - - if (n < 0) { - throw std::invalid_argument("Number of calibration states must be non-negative"); - } - - if (m <= 1) { - throw std::invalid_argument("Number of direction sensors must be at least 2"); - } - - // Compute differential of phi - __Dphi0 = stateActionDiff(__xi_0); - __InnovationLift = __Dphi0.completeOrthogonalDecomposition().pseudoInverse(); -} - -State EqF::stateEstimate() const { - return stateAction(__X_hat, __xi_0); -} - -void EqF::propagation(const Input& u, double dt) { - State state_est = stateEstimate(); - Vector L = lift(state_est, u); - - Matrix Phi_DT = __stateTransitionMatrix(u, dt); - Matrix Bt = __inputMatrixBt(); - - Matrix tempSigma = blockDiag(u.Sigma, - repBlock(1e-9 * Matrix3::Identity(), __n_cal)); - Matrix M_DT = (Bt * tempSigma * Bt.transpose()) * dt; - - __X_hat = __X_hat * G::exp(L * dt); - __Sigma = Phi_DT * __Sigma * Phi_DT.transpose() + M_DT; -} - -bool hasNaN(const Vector3& vec) { - return std::isnan(vec[0]) || std::isnan(vec[1]) || std::isnan(vec[2]); -} - -void EqF::update(const Measurement& y) { - if (y.cal_idx > __n_cal) { - throw std::invalid_argument("Calibration index out of range"); - } - - // Get vector representations for checking - Vector3 y_vec = y.y.d.unitVector(); - Vector3 d_vec = y.d.d.unitVector(); - - // Skip update if any NaN values are present - if (std::isnan(y_vec[0]) || std::isnan(y_vec[1]) || std::isnan(y_vec[2]) || - std::isnan(d_vec[0]) || std::isnan(d_vec[1]) || std::isnan(d_vec[2])) { - return; // Skip this measurement - } - static int update_count = 0; - if (update_count < 5) { - std::cout << "Update " << update_count << ":\n"; - std::cout << "y_vec: " << y_vec.transpose() << "\n"; - std::cout << "d_vec: " << d_vec.transpose() << "\n"; - update_count++; - } - - - - Matrix Ct = __measurementMatrixC(y.d, y.cal_idx); - Vector3 action_result = outputAction(__X_hat.inv(), y.y, y.cal_idx); - Vector3 delta_vec = Rot3::Hat(y.d.d.unitVector()) * action_result; // Ensure this is the right operation - Matrix Dt = __outputMatrixDt(y.cal_idx); - Matrix S = Ct * __Sigma * Ct.transpose() + Dt * y.Sigma * Dt.transpose(); - Matrix K = __Sigma * Ct.transpose() * S.inverse(); - Vector Delta = __InnovationLift * K * delta_vec; - __X_hat = G::exp(Delta) * __X_hat; - __Sigma = (Matrix::Identity(__dof, __dof) - K * Ct) * __Sigma; -} - -Matrix EqF::__stateMatrixA(const Input& u) const { - Matrix3 W0 = velocityAction(__X_hat.inv(), u).W(); - Matrix A1 = Matrix::Zero(6, 6); - - if (COORDINATE == "EXPONENTIAL") { - A1.block<3, 3>(0, 3) = -Matrix3::Identity(); - A1.block<3, 3>(3, 3) = W0; - Matrix A2 = repBlock(W0, __n_cal); - return blockDiag(A1, A2); - } else if (COORDINATE == "NORMAL") { - throw std::runtime_error("Normal coordinate representation is not implemented yet"); - } else { - throw std::invalid_argument("Invalid coordinate representation"); - } -} - -Matrix EqF::__stateTransitionMatrix(const Input& u, double dt) const { - Matrix3 W0 = velocityAction(__X_hat.inv(), u).W(); - Matrix Phi1 = Matrix::Zero(6, 6); - - Matrix3 Phi12 = -dt * (Matrix3::Identity() + (dt / 2) * W0 + ((dt*dt) / 6) * W0 * W0); - Matrix3 Phi22 = Matrix3::Identity() + dt * W0 + ((dt*dt) / 2) * W0 * W0; - - if (COORDINATE == "EXPONENTIAL") { - Phi1.block<3, 3>(0, 0) = Matrix3::Identity(); - Phi1.block<3, 3>(0, 3) = Phi12; - Phi1.block<3, 3>(3, 3) = Phi22; - Matrix Phi2 = repBlock(Phi22, __n_cal); - return blockDiag(Phi1, Phi2); - } else if (COORDINATE == "NORMAL") { - throw std::runtime_error("Normal coordinate representation is not implemented yet"); - } else { - throw std::invalid_argument("Invalid coordinate representation"); - } -} - -Matrix EqF::__inputMatrixBt() const { - if (COORDINATE == "EXPONENTIAL") { - Matrix B1 = blockDiag(__X_hat.A.matrix(), __X_hat.A.matrix()); - Matrix B2; - - for (const auto& B : __X_hat.B) { - if (B2.size() == 0) { - B2 = B.matrix(); - } else { - B2 = blockDiag(B2, B.matrix()); - } - } - - return blockDiag(B1, B2); - } else if (COORDINATE == "NORMAL") { - throw std::runtime_error("Normal coordinate representation is not implemented yet"); - } else { - throw std::invalid_argument("Invalid coordinate representation"); - } -} - -Matrix EqF::__measurementMatrixC(const Direction& d, int idx) const { - Matrix Cc = Matrix::Zero(3, 3 * __n_cal); - - // If the measurement is related to a sensor that has a calibration state - if (idx >= 0) { - // Cc.block<3, 3>(0, 3 * idx) = wedge(d.d.unitVector()); // WRONG - // Set the correct 3x3 block in Cc - Cc.block<3, 3>(0, 3 * idx) = Rot3::Hat(d.d.unitVector()); - } - - Matrix3 wedge_d = Rot3::Hat(d.d.unitVector()); - - // This Matrix concatenation was different from the Python version - // Create the equivalent of: - // Rot3.Hat(d.d.unitVector()) @ np.hstack((Rot3.Hat(d.d.unitVector()), np.zeros((3, 3)), Cc)) - - Matrix temp(3, 6 + 3 * __n_cal); - temp.block<3, 3>(0, 0) = wedge_d; - temp.block<3, 3>(0, 3) = Matrix3::Zero(); - temp.block(0, 6, 3, 3 * __n_cal) = Cc; - - return wedge_d * temp; -} - -Matrix EqF::__outputMatrixDt(int idx) const { - // If the measurement is related to a sensor that has a calibration state - if (idx >= 0) { - if (idx >= static_cast(__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(); - } -} \ No newline at end of file diff --git a/examples/ABC_EQF/EqF.h b/examples/ABC_EQF/EqF.h deleted file mode 100644 index d09099ac1..000000000 --- a/examples/ABC_EQF/EqF.h +++ /dev/null @@ -1,153 +0,0 @@ -// -// Created by darshan on 3/11/25. -// - -#ifndef EQF_H -#define EQF_H -#pragma once - -#include "State.h" -#include "Input.h" -#include "G.h" -#include "Direction.h" -#include "Measurements.h" -#include - -using namespace gtsam; - -/** - * Equivariant Filter (EqF) implementation - */ -class EqF { -private: - int __dof; // Degrees of freedom - int __n_cal; // Number of calibration states - int __n_sensor; // Number of sensors - G __X_hat; // Filter state - Matrix __Sigma; // Error covariance - State __xi_0; // Origin state - Matrix __Dphi0; // Differential of phi at origin - Matrix __InnovationLift; // Innovation lift matrix - -public: - /** - * Initialize EqF - * @param Sigma Initial covariance - * @param n Number of calibration states - * @param m Number of sensors - */ - EqF(const Matrix& Sigma, int n, int m); - - /** - * Return estimated state - * @return Current state estimate - */ - State stateEstimate() const; - - /** - * Propagate the filter state - * @param u Angular velocity measurement - * @param dt Time step - */ - void propagation(const Input& u, double dt); - - /** - * Update the filter state with a measurement - * @param y Direction measurement - */ - void update(const Measurement& y); - -private: - /** - * Return the state matrix A0t (Equation 14a) - * @param u Input - * @return State matrix A0t - */ - Matrix __stateMatrixA(const Input& u) const; - - /** - * Return the state transition matrix Phi (Equation 17) - * @param u Input - * @param dt Time step - * @return State transition matrix Phi - */ - Matrix __stateTransitionMatrix(const Input& u, double dt) const; - - /** - * Return the Input matrix Bt - * @return Input matrix Bt - */ - Matrix __inputMatrixBt() const; - - /** - * Return the measurement matrix C0 (Equation 14b) - * @param d Known direction - * @param idx Calibration index - * @return Measurement matrix C0 - */ - Matrix __measurementMatrixC(const Direction& d, int idx) const; - - /** - * Return the measurement output matrix Dt - * @param idx Calibration index - * @return Measurement output matrix Dt - */ - Matrix __outputMatrixDt(int idx) const; -}; - -// Function declarations for helper functions used by EqF - -/** - * Compute the lift of the system (Theorem 3.8, Equation 7) - * @param xi State - * @param u Input - * @return Lift vector - */ -Vector lift(const State& xi, const Input& u); - -/** - * Action of the symmetry group on the state space (Equation 4) - * @param X Group element - * @param xi State - * @return New state after group action - */ -State stateAction(const G& X, const State& xi); - -/** - * Action of the symmetry group on the input space (Equation 5) - * @param X Group element - * @param u Input - * @return New input after group action - */ -Input velocityAction(const G& X, const Input& u); - -/** - * Action of the symmetry group on the output space (Equation 6) - * @param X Group element - * @param y Direction measurement - * @param idx Calibration index - * @return New direction after group action - */ -Vector3 outputAction(const G& X, const Direction& y, int idx = -1); - -/** - * Local coordinates assuming xi_0 = identity (Equation 9) - * @param e State representing equivariant error - * @return Local coordinates - */ -Vector local_coords(const State& e); - -/** - * Local coordinates inverse assuming xi_0 = identity - * @param eps Local coordinates - * @return Corresponding state - */ -State local_coords_inv(const Vector& eps); - -/** - * Differential of the phi action at E = Id in local coordinates - * @param xi State - * @return Differential matrix - */ -Matrix stateActionDiff(const State& xi); -#endif //EQF_H diff --git a/examples/ABC_EQF/G.cpp b/examples/ABC_EQF/G.cpp deleted file mode 100644 index 1c9684e90..000000000 --- a/examples/ABC_EQF/G.cpp +++ /dev/null @@ -1,61 +0,0 @@ -// -// Created by darshan on 3/11/25. -// -#include "G.h" -#include "utilities.h" -#include - -G::G(const Rot3& A, const Matrix3& a, const std::vector& B) - : A(A), a(a), B(B) {} - -G G::operator*(const G& other) const { - if (B.size() != other.B.size()) { - throw std::invalid_argument("Group elements must have the same number of calibration elements"); - } - - std::vector new_B; - for (size_t i = 0; i < B.size(); i++) { - new_B.push_back(B[i] * other.B[i]); - } - - return G(A * other.A, - a + Rot3::Hat(A.matrix() * Rot3::Vee(other.a)), - new_B); -} - -G G::inv() const { - Matrix3 A_inv = A.inverse().matrix(); - - std::vector B_inv; - for (const auto& b : B) { - B_inv.push_back(b.inverse()); - } - - return G(A.inverse(), - -Rot3::Hat(A_inv * Rot3::Vee(a)), - B_inv); -} - -G G::identity(int n) { - std::vector B(n, Rot3::Identity()); - return G(Rot3::Identity(), Matrix3::Zero(), B); -} - -G G::exp(const Vector& x) { - if (x.size() < 6 || x.size() % 3 != 0) { - throw std::invalid_argument("Wrong size, a vector with size multiple of 3 and at least 6 must be provided"); - } - - int n = (x.size() - 6) / 3; - Rot3 A = Rot3::Expmap(x.head<3>()); - - Vector3 a_vee = Rot3::ExpmapDerivative(-x.head<3>()) * x.segment<3>(3); - Matrix3 a = Rot3::Hat(a_vee); - - std::vector B; - for (int i = 0; i < n; i++) { - B.push_back(Rot3::Expmap(x.segment<3>(6 + 3*i))); - } - - return G(A, a, B); -} \ No newline at end of file diff --git a/examples/ABC_EQF/G.h b/examples/ABC_EQF/G.h deleted file mode 100644 index 397303ac4..000000000 --- a/examples/ABC_EQF/G.h +++ /dev/null @@ -1,63 +0,0 @@ -// -// Created by darshan on 3/11/25. -// - -#ifndef G_H -#define G_H - - -#include -#include -#include -#include - -using namespace gtsam; - -/** - * Symmetry group (SO(3) |x so(3)) x SO(3) x ... x SO(3) - * Each element of the B list is associated with a calibration state - */ -class G { -public: - Rot3 A; // First SO(3) element - Matrix3 a; // so(3) element (skew-symmetric matrix) - std::vector B; // List of SO(3) elements for calibration - - /** - * Initialize the symmetry group G - * @param A SO3 element - * @param a so(3) element (skew symmetric matrix) - * @param B list of SO3 elements - */ - G(const Rot3& A = Rot3::Identity(), - const Matrix3& a = Matrix3::Zero(), - const std::vector& B = std::vector()); - - /** - * Define the group operation (multiplication) - * @param other Another group element - * @return The product of this and other - */ - G operator*(const G& other) const; - - /** - * Return the inverse element of the symmetry group - * @return The inverse of this group element - */ - G inv() const; - - /** - * Return the identity of the symmetry group - * @param n Number of calibration elements - * @return The identity element with n calibration components - */ - static G identity(int n); - - /** - * Return a group element X given by X = exp(x) - * @param x Vector representation of Lie algebra element - * @return Group element given by the exponential of x - */ - static G exp(const Vector& x); -}; -#endif //G_H diff --git a/examples/ABC_EQF/Input.cpp b/examples/ABC_EQF/Input.cpp deleted file mode 100644 index 488102cdb..000000000 --- a/examples/ABC_EQF/Input.cpp +++ /dev/null @@ -1,30 +0,0 @@ -// -// Created by darshan on 3/11/25. -// -#include "Input.h" -#include "utilities.h" -#include -#include -#include "gtsam/geometry/Rot3.h" - -Input::Input(const Vector3& w, const Matrix& Sigma) - : w(w), Sigma(Sigma) { - if (Sigma.rows() != 6 || Sigma.cols() != 6) { - throw std::invalid_argument("Input measurement noise covariance must be 6x6"); - } - - // Check positive semi-definite - Eigen::SelfAdjointEigenSolver eigensolver(Sigma); - if (eigensolver.eigenvalues().minCoeff() < -1e-10) { - throw std::invalid_argument("Covariance matrix must be semi-positive definite"); - } -} - -Matrix3 Input::W() const { - return Rot3::Hat(w); -} - -Input Input::random() { - Vector3 w = Vector3::Random(); - return Input(w, Matrix::Identity(6, 6)); -} \ No newline at end of file diff --git a/examples/ABC_EQF/Input.h b/examples/ABC_EQF/Input.h deleted file mode 100644 index 437d63b14..000000000 --- a/examples/ABC_EQF/Input.h +++ /dev/null @@ -1,41 +0,0 @@ -// -// Created by darshan on 3/11/25. -// - -#ifndef INPUT_H -#define INPUT_H - -#include -#include - -using namespace gtsam; - -/** - * Input class for the Biased Attitude System - */ -class Input { -public: - Vector3 w; // Angular velocity - Matrix Sigma; // Noise covariance - - /** - * Initialize Input - * @param w Angular velocity (3-vector) - * @param Sigma Noise covariance (6x6 matrix) - */ - Input(const Vector3& w, const Matrix& Sigma); - - /** - * Return the Input as a skew-symmetric matrix - * @return w as a skew-symmetric matrix - */ - Matrix3 W() const; - - /** - * Return a random angular velocity - * @return A random angular velocity as Input element - */ - static Input random(); -}; - -#endif //INPUT_H diff --git a/examples/ABC_EQF/Measurements.cpp b/examples/ABC_EQF/Measurements.cpp deleted file mode 100644 index f1bd987ab..000000000 --- a/examples/ABC_EQF/Measurements.cpp +++ /dev/null @@ -1,17 +0,0 @@ -// -// Created by darshan on 3/11/25. -// -#include "Measurements.h" -#include -#include - -Measurement::Measurement(const Vector3& y_vec, const Vector3& d_vec, - const Matrix3& Sigma, int i) - : y(y_vec), d(d_vec), Sigma(Sigma), cal_idx(i) { - - // Check positive semi-definite - Eigen::SelfAdjointEigenSolver eigensolver(Sigma); - if (eigensolver.eigenvalues().minCoeff() < -1e-10) { - throw std::invalid_argument("Covariance matrix must be semi-positive definite"); - } -} \ No newline at end of file diff --git a/examples/ABC_EQF/Measurements.h b/examples/ABC_EQF/Measurements.h deleted file mode 100644 index b74cc538c..000000000 --- a/examples/ABC_EQF/Measurements.h +++ /dev/null @@ -1,36 +0,0 @@ -// -// Created by darshan on 3/11/25. -// - -#ifndef MEASUREMENTS_H -#define MEASUREMENTS_H - - -#include "Direction.h" -#include - -using namespace gtsam; - -/** - * Measurement class - * cal_idx is an index corresponding to the calibration related to the measurement - * cal_idx = -1 indicates the measurement is from a calibrated sensor - */ -class Measurement { -public: - Direction y; // Measurement direction in sensor frame - Direction d; // Known direction in global frame - Matrix3 Sigma; // Covariance matrix of the measurement - int cal_idx = -1; // Calibration index (-1 for calibrated sensor) - - /** - * Initialize measurement - * @param y_vec Direction measurement in sensor frame - * @param d_vec Known direction in global frame - * @param Sigma Measurement noise covariance - * @param i Calibration index (-1 for calibrated sensor) - */ - Measurement(const Vector3& y_vec, const Vector3& d_vec, - const Matrix3& Sigma, int i = -1); -}; -#endif //MEASUREMENTS_H diff --git a/examples/ABC_EQF/State.cpp b/examples/ABC_EQF/State.cpp deleted file mode 100644 index 90dabfbda..000000000 --- a/examples/ABC_EQF/State.cpp +++ /dev/null @@ -1,12 +0,0 @@ -// -// Created by darshan on 3/11/25. -// -#include "State.h" - -State::State(const Rot3& R, const Vector3& b, const std::vector& S) - : R(R), b(b), S(S) {} - -State State::identity(int n) { - std::vector calibrations(n, Rot3::Identity()); - return State(Rot3::Identity(), Vector3::Zero(), calibrations); -} \ No newline at end of file diff --git a/examples/ABC_EQF/State.h b/examples/ABC_EQF/State.h deleted file mode 100644 index 6c8b9c5a6..000000000 --- a/examples/ABC_EQF/State.h +++ /dev/null @@ -1,29 +0,0 @@ -// -// Created by darshan on 3/11/25. -// - -#ifndef STATE_H -#define STATE_H - -#include -#include -#include - -using namespace gtsam; - -/** - * State class representing the state of the Biased Attitude System - */ -class State { -public: - Rot3 R; // Attitude rotation matrix R - Vector3 b; // Gyroscope bias b - std::vector S; // Sensor calibrations S - - State(const Rot3& R = Rot3::Identity(), - const Vector3& b = Vector3::Zero(), - const std::vector& S = std::vector()); - - static State identity(int n); -}; -#endif //STATE_H diff --git a/examples/ABC_EQF/main.cpp b/examples/ABC_EQF/main.cpp deleted file mode 100644 index b21eae44a..000000000 --- a/examples/ABC_EQF/main.cpp +++ /dev/null @@ -1,194 +0,0 @@ -// -// Created by darshan on 3/11/25. -// -#include "EqF.h" -#include "State.h" -#include "Input.h" -#include "Direction.h" -#include "Measurements.h" -#include "Data.h" -#include "runEQF_withcsv.h" -#include "utilities.h" -#include -#include -#include -#include -#include -#include -#include - - -using namespace std; -using namespace gtsam; - -// Simplified data loading function - in a real application, implement proper CSV parsing -std::vector loadSimulatedData() { - std::vector data_list; - - double t = 0.0; - double dt = 0.01; - - // Number of data points - int num_points = 100; - - // Set up one calibration state - int n_cal = 1; - - for (int i = 0; i < num_points; i++) { - t += dt; - - // Create a simple sinusoidal trajectory - double angle = 0.1 * sin(t); - Rot3 R = Rot3::Rz(angle); - - // Create a bias - Vector3 b(0.01, 0.02, 0.03); - - // Create a calibration - std::vector S; - S.push_back(Rot3::Ry(0.05)); - - // State - State xi(R, b, S); - - // Input (angular velocity) - Vector3 w(0.1 * cos(t), 0.05 * sin(t), 0.02); - Matrix Sigma_u = Matrix::Identity(6, 6) * 0.01; - Input u(w, Sigma_u); - - // Measurements - std::vector measurements; - - // Measurement 1 - from uncalibrated sensor - Vector3 d1_vec = Vector3(1, 0, 0).normalized(); // Known direction in global frame - Vector3 y1_vec = S[0].inverse().matrix() * R.inverse().matrix() * d1_vec; // Direction in sensor frame - Matrix3 Sigma1 = Matrix3::Identity() * 0.01; - measurements.push_back(Measurement(y1_vec, d1_vec, Sigma1, 0)); // cal_idx = 0 - - // Measurement 2 - from calibrated sensor - Vector3 d2_vec = Vector3(0, 1, 0).normalized(); // Known direction in global frame - Vector3 y2_vec = R.inverse().matrix() * d2_vec; // Direction in sensor frame - Matrix3 Sigma2 = Matrix3::Identity() * 0.01; - measurements.push_back(Measurement(y2_vec, d2_vec, Sigma2, -1)); // cal_idx = -1 - - // Add to data list - data_list.push_back(Data(xi, n_cal, u, measurements, 2, t, dt)); - } - - return data_list; -} - -void runSimulation(EqF& filter, const std::vector& data) { - std::cout << "Starting simulation with " << data.size() << " data points..." << std::endl; - - // Track time for performance measurement - auto start = std::chrono::high_resolution_clock::now(); - - // Store results for analysis - std::vector times; - std::vector attitude_errors; - std::vector bias_errors; - std::vector calibration_errors; - - for (const auto& d : data) { - // Propagation - try { - filter.propagation(d.u, d.dt); - } catch (const std::exception& e) { - std::cerr << "Propagation error at t=" << d.t << ": " << e.what() << std::endl; - continue; - } - - // Update with measurements - for (const auto& y : d.y) { - try { - if (!std::isnan(y.y.d.unitVector().norm()) && !std::isnan(y.d.d.unitVector().norm())) { - filter.update(y); - } - } catch (const std::exception& e) { - std::cerr << "Update error at t=" << d.t << ": " << e.what() << std::endl; - } - } - - // Get state estimate - State estimate = filter.stateEstimate(); - - // Compute errors - Vector3 att_error = Rot3::Logmap(d.xi.R.between(estimate.R)); - Vector3 bias_error = estimate.b - d.xi.b; - Vector3 cal_error = Vector3::Zero(); - if (!d.xi.S.empty() && !estimate.S.empty()) { - cal_error = Rot3::Logmap(d.xi.S[0].between(estimate.S[0])); - } - - // Store results - times.push_back(d.t); - attitude_errors.push_back(att_error); - bias_errors.push_back(bias_error); - calibration_errors.push_back(cal_error); - - // Print some info - if (d.t < 0.1 || fmod(d.t, 1.0) < d.dt) { - std::cout << "Time: " << d.t - << ", Attitude error (deg): " << (att_error.norm() * 180.0/M_PI) - << ", Bias error: " << bias_error.norm() - << ", Calibration error (deg): " << (cal_error.norm() * 180.0/M_PI) - << std::endl; - } - } - - auto end = std::chrono::high_resolution_clock::now(); - std::chrono::duration elapsed = end - start; - std::cout << "Simulation completed in " << elapsed.count() << " seconds" << std::endl; - - // Print summary statistics - double avg_att_error = 0.0; - double avg_bias_error = 0.0; - double avg_cal_error = 0.0; - - for (size_t i = 0; i < times.size(); i++) { - avg_att_error += attitude_errors[i].norm(); - avg_bias_error += bias_errors[i].norm(); - avg_cal_error += calibration_errors[i].norm(); - } - - avg_att_error /= times.size(); - avg_bias_error /= times.size(); - avg_cal_error /= times.size(); - - std::cout << "Average attitude error (deg): " << (avg_att_error * 180.0/M_PI) << std::endl; - std::cout << "Average bias error: " << avg_bias_error << std::endl; - std::cout << "Average calibration error (deg): " << (avg_cal_error * 180.0/M_PI) << std::endl; -} - - - - -int main(int argc, char** argv) { - std::cout << "ABC-EqF: Attitude-Bias-Calibration Equivariant Filter" << std::endl; - std::cout << "========================================================" << std::endl; - - std::string csvFilePath; - - // Try to find the EQFdata file in the GTSAM examples directory - try { - // Look specifically for EQFdata.csv in GTSAM examples - csvFilePath = findExampleDataFile("EqFdata.csv"); - std::cout << "Using GTSAM example data file: " << csvFilePath << std::endl; - } catch (const std::exception& e) { - std::cerr << "Error: Could not find EqFdata.csv in GTSAM examples directory" << std::endl; - std::cerr << e.what() << std::endl; - return 1; - } - - try { - // Run with CSV data - runEqFWithCSVData(csvFilePath); - } catch (const std::exception& e) { - std::cerr << "Error: " << e.what() << std::endl; - return 1; - } - - std::cout << "Done." << std::endl; - return 0; -} \ No newline at end of file diff --git a/examples/ABC_EQF/runEQF_withcsv.h b/examples/ABC_EQF/runEQF_withcsv.h deleted file mode 100644 index d31baffe0..000000000 --- a/examples/ABC_EQF/runEQF_withcsv.h +++ /dev/null @@ -1,683 +0,0 @@ -// -// Created by darshan on 3/17/25. -// - -#ifndef RUNEQF_WITHCSV_H -#define RUNEQF_WITHCSV_H - -// -// Created by darshan on 3/17/25. -// -#include "Data.h" -#include "State.h" -#include "Input.h" -#include "Direction.h" -#include "Measurements.h" -#include "utilities.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/** - * Load data from CSV file into a vector of Data objects for the EqF - * - * CSV format: - * - t: Time - * - q_w, q_x, q_y, q_z: True attitude quaternion - * - b_x, b_y, b_z: True bias - * - cq_w_0, cq_x_0, cq_y_0, cq_z_0: True calibration quaternion - * - w_x, w_y, w_z: Angular velocity measurements - * - std_w_x, std_w_y, std_w_z: Angular velocity measurement standard deviations - * - std_b_x, std_b_y, std_b_z: Bias process noise standard deviations - * - y_x_0, y_y_0, y_z_0, y_x_1, y_y_1, y_z_1: Direction measurements - * - std_y_x_0, std_y_y_0, std_y_z_0, std_y_x_1, std_y_y_1, std_y_z_1: Direction measurement standard deviations - * - d_x_0, d_y_0, d_z_0, d_x_1, d_y_1, d_z_1: Reference directions - * - * @param filename Path to the CSV file - * @param startRow First row to load (default: 0) - * @param maxRows Maximum number of rows to load (default: all) - * @param downsample Downsample factor (default: 1, which means no downsampling) - * @return Vector of Data objects - */ -inline std::vector loadDataFromCSV(const std::string& filename, - int startRow = 0, - int maxRows = -1, - int downsample = 1) { - std::vector data_list; - std::ifstream file(filename); - - if (!file.is_open()) { - throw std::runtime_error("Failed to open file: " + filename); - } - - std::string line; - int lineNumber = 0; - int rowCount = 0; - double prevTime = 0.0; - - // Skip header - std::getline(file, line); - lineNumber++; - - // Skip to startRow - while (lineNumber < startRow && std::getline(file, line)) { - lineNumber++; - } - - // Read data - while (std::getline(file, line) && (maxRows == -1 || rowCount < maxRows)) { - lineNumber++; - - // Apply downsampling - if ((lineNumber - startRow - 1) % downsample != 0) { - continue; - } - - std::istringstream ss(line); - std::string token; - std::vector values; - - // Parse line into values - while (std::getline(ss, token, ',')) { - try { - values.push_back(std::stod(token)); - } catch (const std::exception& e) { - std::cerr << "Error parsing value at line " << lineNumber << ": " << token << std::endl; - values.push_back(0.0); // Use default value - } - } - - // Check if we have enough values - if (values.size() < 39) { - std::cerr << "Warning: Line " << lineNumber << " has only " << values.size() - << " values, expected 39. Skipping." << std::endl; - continue; - } - - // Extract values - double t = values[0]; - double dt = (rowCount == 0) ? 0.0 : t - prevTime; - prevTime = t; - - // Create ground truth state - Quaternion quat(values[1], values[2], values[3], values[4]); // w, x, y, z - Rot3 R = Rot3(quat); - - Vector3 b(values[5], values[6], values[7]); - - Quaternion calQuat(values[8], values[9], values[10], values[11]); // w, x, y, z - std::vector 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(y0, d0, covY0, 0)); - - // Second measurement (calibrated sensor, cal_idx = -1) - Vector3 y1(values[24], values[25], values[26]); - Vector3 d1(values[36], values[37], values[38]); - - // Normalize vectors if needed - if (abs(y1.norm() - 1.0) > 1e-5) y1.normalize(); - if (abs(d1.norm() - 1.0) > 1e-5) d1.normalize(); - - // Measurement covariance - Matrix3 covY1 = Matrix3::Zero(); - covY1(0, 0) = values[30] * values[30]; // std_y_x_1^2 - covY1(1, 1) = values[31] * values[31]; // std_y_y_1^2 - covY1(2, 2) = values[32] * values[32]; // std_y_z_1^2 - - // Create measurement - measurements.push_back(Measurement(y1, d1, covY1, -1)); - - // Create Data object and add to list - data_list.push_back(Data(xi, 1, u, measurements, 2, t, dt)); - - rowCount++; - } - - std::cout << "Loaded " << data_list.size() << " data points from CSV file." << std::endl; - - return data_list; -} - -/** - * Process Data objects with the EqF filter - * - * @param filter EqF filter to use - * @param data_list Vector of Data objects - * @param saveResults Whether to save results to a file - * @param resultFilename Filename to save results to - */ -inline void printDataPoint(const Data& data, int index) { - std::cout << "Data[" << index << "] @ t=" << data.t << ", dt=" << data.dt << std::endl; - - // Print angular velocity - std::cout << " ω = [" << data.u.w[0] << ", " << data.u.w[1] << ", " << data.u.w[2] << "]" << std::endl; - - // Print measurements - for (size_t i = 0; i < data.y.size(); i++) { - const Measurement& meas = data.y[i]; - // Use the unitVector() method to get a Vector3 from a Unit3 object - Vector3 y_vec = meas.y.d.unitVector(); - Vector3 d_vec = meas.d.d.unitVector(); - std::cout << " y" << i << " = [" << y_vec[0] << ", " << y_vec[1] << ", " << y_vec[2] << "]" << std::endl; - std::cout << " d" << i << " = [" << d_vec[0] << ", " << d_vec[1] << ", " << d_vec[2] << "]" << std::endl; - } - - std::cout << std::endl; -} - - -// Function to print sample data points -inline void printDataSamples(const std::vector& data_list, int count = 3) { - int total = data_list.size(); - - std::cout << "\n=== First " << count << " Data Points ===" << std::endl; - for (int i = 0; i < std::min(count, total); i++) { - printDataPoint(data_list[i], i); - } - - if (total > 2*count) { - std::cout << "\n... (" << (total - 2*count) << " points omitted) ...\n" << std::endl; - - std::cout << "=== Last " << count << " Data Points ===" << std::endl; - for (int i = std::max(count, total - count); i < total; i++) { - printDataPoint(data_list[i], i); - } - } -} - -// Function to validate data -inline bool validateData(const std::vector& data_list) { - if (data_list.empty()) { - std::cerr << "ERROR: No data loaded from CSV" << std::endl; - return false; - } - - std::cout << "Validating " << data_list.size() << " data points..." << std::endl; - - // Track statistics - int invalid_count = 0; - - // Open a log file to record detailed issues - std::ofstream logFile("data_validation.log"); - logFile << "Data Validation Report" << std::endl; - logFile << "--------------------" << std::endl; - - for (size_t i = 0; i < data_list.size(); ++i) { - const Data& data = data_list[i]; - bool point_valid = true; - - // Check time and dt - if (std::isnan(data.t) || std::isnan(data.dt)) { - logFile << "Point " << i << ": Invalid time values (t=" << data.t - << ", dt=" << data.dt << ")" << std::endl; - point_valid = false; - } - - // Check ground truth state for NaN - using isnan directly on components - const auto& R_matrix = data.xi.R.matrix(); - bool R_has_nan = false; - for (int r = 0; r < 3; r++) { - for (int c = 0; c < 3; c++) { - if (std::isnan(R_matrix(r, c))) { - R_has_nan = true; - break; - } - } - } - - if (R_has_nan) { - logFile << "Point " << i << ": NaN in ground truth attitude matrix" << std::endl; - point_valid = false; - } - - // Check bias vector for NaN - bool b_has_nan = false; - for (int j = 0; j < 3; j++) { - if (std::isnan(data.xi.b[j])) { - b_has_nan = true; - break; - } - } - - if (b_has_nan) { - logFile << "Point " << i << ": NaN in ground truth bias vector" << std::endl; - point_valid = false; - } - - // Check calibration matrices for NaN - for (size_t j = 0; j < data.xi.S.size(); ++j) { - const auto& S_matrix = data.xi.S[j].matrix(); - bool S_has_nan = false; - for (int r = 0; r < 3; r++) { - for (int c = 0; c < 3; c++) { - if (std::isnan(S_matrix(r, c))) { - S_has_nan = true; - break; - } - } - } - - if (S_has_nan) { - logFile << "Point " << i << ": NaN in ground truth calibration matrix " - << j << std::endl; - point_valid = false; - } - } - - // Check input for NaN - bool w_has_nan = false; - for (int j = 0; j < 3; j++) { - if (std::isnan(data.u.w[j])) { - w_has_nan = true; - break; - } - } - - if (w_has_nan) { - logFile << "Point " << i << ": NaN in angular velocity" << std::endl; - point_valid = false; - } - - // Check measurements - for (size_t j = 0; j < data.y.size(); ++j) { - const Measurement& meas = data.y[j]; - - // Get the Vector3 representations to check them - Vector3 y_vec = meas.y.d.unitVector(); - Vector3 d_vec = meas.d.d.unitVector(); - - // Check measurement vector for NaN - bool y_has_nan = false; - bool d_has_nan = false; - - for (int k = 0; k < 3; k++) { - if (std::isnan(y_vec[k])) { - y_has_nan = true; - break; - } - if (std::isnan(d_vec[k])) { - d_has_nan = true; - break; - } - } - - if (y_has_nan) { - logFile << "Point " << i << ", Meas " << j << ": NaN in measurement vector" << std::endl; - point_valid = false; - } - - if (d_has_nan) { - logFile << "Point " << i << ", Meas " << j << ": NaN in reference direction" << std::endl; - point_valid = false; - } - - // Calculate norm using Vector3 norms - double y_norm = y_vec.norm(); - double d_norm = d_vec.norm(); - - if (std::abs(y_norm - 1.0) > 1e-5) { - logFile << "Point " << i << ", Meas " << j - << ": Measurement vector not normalized. Norm = " << y_norm << std::endl; - point_valid = false; - } - - if (std::abs(d_norm - 1.0) > 1e-5) { - logFile << "Point " << i << ", Meas " << j - << ": Reference direction not normalized. Norm = " << d_norm << std::endl; - point_valid = false; - } - } - - if (!point_valid) { - invalid_count++; - - // Print first few invalid points to console - if (invalid_count <= 5) { - std::cerr << "Invalid data at point " << i << " (t=" << data.t << ")" << std::endl; - } - } - } - - // Close the log - logFile << std::endl << "Summary: " << invalid_count << " invalid data points out of " - << data_list.size() << std::endl; - logFile.close(); - - - - - - - - - - // Print summary - std::cout << "Data validation complete. " << invalid_count << " invalid points found." << std::endl; - if (invalid_count > 0) { - std::cout << "See data_validation.log for details." << std::endl; - } - - return (invalid_count == 0); -} - -inline void processDataWithEqF(EqF& filter, - const std::vector& data_list, - bool saveResults = false, - const std::string& resultFilename = "eqf_results.csv") { - std::ofstream resultFile; - if (saveResults) { - resultFile.open(resultFilename); - if (!resultFile.is_open()) { - throw std::runtime_error("Failed to open result file: " + resultFilename); - } - - // Write header - now adding roll, pitch, yaw columns for estimated and true values - resultFile << "t,"; - // Estimated state quaternion - resultFile << "est_qw,est_qx,est_qy,est_qz,"; - // Estimated bias - resultFile << "est_bx,est_by,est_bz,"; - // Estimated calibration quaternion - resultFile << "est_cqw,est_cqx,est_cqy,est_cqz,"; - // True state quaternion - resultFile << "true_qw,true_qx,true_qy,true_qz,"; - // True bias - resultFile << "true_bx,true_by,true_bz,"; - // True calibration quaternion - resultFile << "true_cqw,true_cqx,true_cqy,true_cqz,"; - // Add Euler angles for estimated state - resultFile << "est_roll,est_pitch,est_yaw,"; - // Add Euler angles for true state - resultFile << "true_roll,true_pitch,true_yaw,"; - // Add Euler angles for estimated calibration - resultFile << "est_cal_roll,est_cal_pitch,est_cal_yaw,"; - // Add Euler angles for true calibration - resultFile << "true_cal_roll,true_cal_pitch,true_cal_yaw"; - resultFile << std::endl; - } - - std::cout << "Processing data with EqF..." << std::endl; - - // Track time for performance measurement - auto start = std::chrono::high_resolution_clock::now(); - - // Store error metrics - std::vector att_errors; - std::vector bias_errors; - std::vector cal_errors; - - int total_measurements = 0; - int valid_measurements = 0; - int invalid_measurements = 0; - - for (size_t i = 0; i < data_list.size(); i++) { - const Data& data = data_list[i]; - - // Propagation - filter.propagation(data.u, data.dt); - - // Update with measurements - for (const auto& y : data.y) { - total_measurements++; - - // Check for NaN values in measurement vectors - bool has_nan = false; - Vector3 y_vec = y.y.d.unitVector(); - Vector3 d_vec = y.d.d.unitVector(); - - for (int j = 0; j < 3; j++) { - if (std::isnan(y_vec[j]) || std::isnan(d_vec[j])) { - has_nan = true; - break; - } - } - - if (!has_nan) { - try { - filter.update(y); - valid_measurements++; - } catch (const std::exception& e) { - std::cerr << "Error updating at t=" << data.t << ": " << e.what() << std::endl; - invalid_measurements++; - } - } else { - invalid_measurements++; - } - } - - // Get state estimate - State estimate = filter.stateEstimate(); - - // Compute errors - Vector3 att_error = Rot3::Logmap(data.xi.R.between(estimate.R)); - Vector3 bias_error = estimate.b - data.xi.b; - Vector3 cal_error = Vector3::Zero(); - if (!data.xi.S.empty() && !estimate.S.empty()) { - cal_error = Rot3::Logmap(data.xi.S[0].between(estimate.S[0])); - } - - // Store errors - att_errors.push_back(att_error.norm()); - bias_errors.push_back(bias_error.norm()); - cal_errors.push_back(cal_error.norm()); - - // Save results - if (saveResults) { - // Extract quaternions - Quaternion est_q = estimate.R.toQuaternion(); - Quaternion true_q = data.xi.R.toQuaternion(); - - // Extract Euler angles (roll, pitch, yaw) from estimated rotation - Vector3 est_rpy = estimate.R.rpy(); - // Convert to degrees for easier comparison - Vector3 est_rpy_deg = est_rpy * 180.0 / M_PI; - - // Extract Euler angles from true rotation - Vector3 true_rpy = data.xi.R.rpy(); - // Convert to degrees - Vector3 true_rpy_deg = true_rpy * 180.0 / M_PI; - - // Get calibration quaternions and Euler angles - Quaternion est_cal_q, true_cal_q; - Vector3 est_cal_rpy_deg = Vector3::Zero(); - Vector3 true_cal_rpy_deg = Vector3::Zero(); - - if (!estimate.S.empty() && !data.xi.S.empty()) { - est_cal_q = estimate.S[0].toQuaternion(); - true_cal_q = data.xi.S[0].toQuaternion(); - - // Get Euler angles for calibrations - Vector3 est_cal_rpy = estimate.S[0].rpy(); - est_cal_rpy_deg = est_cal_rpy * 180.0 / M_PI; - - Vector3 true_cal_rpy = data.xi.S[0].rpy(); - true_cal_rpy_deg = true_cal_rpy * 180.0 / M_PI; - } else { - est_cal_q = Quaternion(1, 0, 0, 0); // Identity quaternion - true_cal_q = Quaternion(1, 0, 0, 0); - } - - // Write to file - resultFile << data.t << ","; - // Estimated quaternion - resultFile << est_q.w() << "," << est_q.x() << "," << est_q.y() << "," << est_q.z() << ","; - // Estimated bias - resultFile << estimate.b[0] << "," << estimate.b[1] << "," << estimate.b[2] << ","; - // Estimated calibration quaternion - resultFile << est_cal_q.w() << "," << est_cal_q.x() << "," << est_cal_q.y() << "," << est_cal_q.z() << ","; - // True quaternion - resultFile << true_q.w() << "," << true_q.x() << "," << true_q.y() << "," << true_q.z() << ","; - // True bias - resultFile << data.xi.b[0] << "," << data.xi.b[1] << "," << data.xi.b[2] << ","; - // True calibration quaternion - resultFile << true_cal_q.w() << "," << true_cal_q.x() << "," << true_cal_q.y() << "," << true_cal_q.z() << ","; - - // Add Euler angles (in degrees) for estimated state - resultFile << est_rpy_deg[0] << "," << est_rpy_deg[1] << "," << est_rpy_deg[2] << ","; - // Add Euler angles (in degrees) for true state - resultFile << true_rpy_deg[0] << "," << true_rpy_deg[1] << "," << true_rpy_deg[2] << ","; - // Add Euler angles (in degrees) for estimated calibration - resultFile << est_cal_rpy_deg[0] << "," << est_cal_rpy_deg[1] << "," << est_cal_rpy_deg[2] << ","; - // Add Euler angles (in degrees) for true calibration - resultFile << true_cal_rpy_deg[0] << "," << true_cal_rpy_deg[1] << "," << true_cal_rpy_deg[2]; - - resultFile << std::endl; - } - - // Print progress - if (i % 1000 == 0 || i == data_list.size() - 1) { - std::cout << "Processed " << i+1 << "/" << data_list.size() - << " (" << (100.0 * (i+1) / data_list.size()) << "%) "; - std::cout << "Attitude error: " << (att_error.norm() * 180.0/M_PI) << " deg, "; - std::cout << "Bias error: " << bias_error.norm() << ", "; - std::cout << "Calibration error: " << (cal_error.norm() * 180.0/M_PI) << " deg" << std::endl; - } - } - - auto end = std::chrono::high_resolution_clock::now(); - std::chrono::duration elapsed = end - start; - - // Calculate average errors - double avg_att_error = 0.0; - double avg_bias_error = 0.0; - double avg_cal_error = 0.0; - - if (!att_errors.empty()) { - avg_att_error = std::accumulate(att_errors.begin(), att_errors.end(), 0.0) / att_errors.size(); - avg_bias_error = std::accumulate(bias_errors.begin(), bias_errors.end(), 0.0) / bias_errors.size(); - avg_cal_error = std::accumulate(cal_errors.begin(), cal_errors.end(), 0.0) / cal_errors.size(); - } - - std::cout << std::endl; - std::cout << "EqF Processing completed in " << elapsed.count() << " seconds" << std::endl; - std::cout << "Average attitude error: " << (avg_att_error * 180.0/M_PI) << " deg" << std::endl; - std::cout << "Average bias error: " << avg_bias_error << std::endl; - std::cout << "Average calibration error: " << (avg_cal_error * 180.0/M_PI) << " deg" << std::endl; - std::cout << "Total measurements: " << total_measurements << std::endl; - std::cout << "Valid measurements processed: " << valid_measurements << std::endl; - std::cout << "Invalid measurements skipped: " << invalid_measurements << std::endl; - - if (saveResults) { - resultFile.close(); - std::cout << "Results saved to " << resultFilename << std::endl; - } -} - - - - - -inline void runEqFWithCSVData(const std::string& filename) { - try { - // Load data from CSV file with optional parameters - int startRow = 0; - int maxRows = -1; - int downsample = 1; - - std::vector data = loadDataFromCSV(filename, startRow, maxRows, downsample); - - if (data.empty()) { - std::cerr << "No data loaded from CSV file." << std::endl; - return; - } - - // Print sample data points to inspect the loaded data - std::cout << "Data loaded, displaying samples..." << std::endl; - printDataSamples(data); - - // Validate the data to check for issues - std::cout << "Validating data integrity..." << std::endl; - bool dataValid = validateData(data); - - if (!dataValid) { - std::cout << "Warning: Data validation found issues." << std::endl; - std::string proceed; - std::cout << "Do you want to proceed anyway? (y/n): "; - std::cin >> proceed; - if (proceed != "y" && proceed != "Y") { - std::cout << "Operation cancelled by user." << std::endl; - return; - } - } - - // Initialize EqF filter - int n_cal = 1; // Number of calibration states (from the data) - int n_sensors = 2; // Number of sensors (from the data) - - // Initial covariance - Matrix initialSigma = Matrix::Identity(6 + 3*n_cal, 6 + 3*n_cal); - initialSigma.diagonal().head<3>() = Vector3::Constant(0.1); // Reduced attitude uncertainty - initialSigma.diagonal().segment<3>(3) = Vector3::Constant(0.01); // Reduced bias uncertainty - initialSigma.diagonal().tail<3>() = Vector3::Constant(0.1); // Reduced calibration uncertainty - - // Create filter - EqF filter(initialSigma, n_cal, n_sensors); - - // Initialize filter state from the first ground truth if possible - if (!data.empty()) { - // You'll need to add a method to your EqF class to set the initial state - // Something like: - // filter.setInitialState(data[0].xi); - - // If you don't have such a method, you can print the first ground truth - // to see if it makes sense - std::cout << "First ground truth state:" << std::endl; - std::cout << "Attitude: " << data[0].xi.R.matrix() << std::endl; - std::cout << "Bias: " << data[0].xi.b.transpose() << std::endl; - std::cout << "Calibration: " << data[0].xi.S[0].matrix() << std::endl; - } - - // Process data with the filter and save results - processDataWithEqF(filter, data, true, "eqf_results.csv"); - - } catch (const std::exception& e) { - std::cerr << "Error: " << e.what() << std::endl; - } -} -/** - * Example usage function to demonstrate how to use the data loader with the EqF - */ - -#endif //RUNEQF_WITHCSV_H diff --git a/examples/ABC_EQF/utilities.cpp b/examples/ABC_EQF/utilities.cpp deleted file mode 100644 index 5166cb0d6..000000000 --- a/examples/ABC_EQF/utilities.cpp +++ /dev/null @@ -1,57 +0,0 @@ -// -// Created by darshan on 3/11/25. -// -#include "utilities.h" -#include - -// Global configuration -const std::string COORDINATE = "EXPONENTIAL"; // Alternative: "NORMAL" - - -bool checkNorm(const Vector3& x, double tol) { - return abs(x.norm() - 1) < tol || std::isnan(x.norm()); -} - -Matrix blockDiag(const Matrix& A, const Matrix& B) { - if (A.size() == 0) { - return B; - } else if (B.size() == 0) { - return A; - } else { - Matrix result(A.rows() + B.rows(), A.cols() + B.cols()); - result.setZero(); - result.block(0, 0, A.rows(), A.cols()) = A; - result.block(A.rows(), A.cols(), B.rows(), B.cols()) = B; - return result; - } -} - -Matrix repBlock(const Matrix& A, int n) { - if (n <= 0) return Matrix(); - - Matrix result = A; - for (int i = 1; i < n; i++) { - result = blockDiag(result, A); - } - return result; -} - -Matrix numericalDifferential(std::function 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; -} \ No newline at end of file diff --git a/examples/ABC_EQF/utilities.h b/examples/ABC_EQF/utilities.h deleted file mode 100644 index 50bbf88aa..000000000 --- a/examples/ABC_EQF/utilities.h +++ /dev/null @@ -1,28 +0,0 @@ -// -// Created by darshan on 3/11/25. -// - -#ifndef UTILITIES_H -#define UTILITIES_H - -#pragma once - -#include -#include -#include -#include - -using namespace gtsam; - -// Global configuration -extern const std::string COORDINATE; // "EXPONENTIAL" or "NORMAL" - -/** - * Utility functions - */ -Matrix3 wedge(const Vector3& vec); -bool checkNorm(const Vector3& x, double tol = 1e-3); -Matrix blockDiag(const Matrix& A, const Matrix& B); -Matrix repBlock(const Matrix& A, int n); -Matrix numericalDifferential(std::function f, const Vector& x); -#endif //UTILITIES_H diff --git a/examples/ABC_EQF_Demo.cpp b/examples/ABC_EQF_Demo.cpp new file mode 100644 index 000000000..6b0f4389d --- /dev/null +++ b/examples/ABC_EQF_Demo.cpp @@ -0,0 +1,89 @@ +/** + * @file ABC_EQF_Demo.cpp + * @brief Demonstration of the full Attitude-Bias-Calibration Equivariant Filter + * + * This demo shows the Equivariant Filter (EqF) for attitude estimation + * with both gyroscope bias and sensor extrinsic calibration, based on the paper: + * "Overcoming Bias: Equivariant Filter Design for Biased Attitude Estimation + * with Online Calibration" by Fornasier et al. + * Authors: Darshan Rajasekaran & Jennifer Oum + */ + +#include "ABC_EQF.h" + +// Use namespace for convenience +using namespace abc_eqf_lib; +using namespace gtsam; + +/** + * Main function for the EqF demo + * @param argc Number of arguments + * @param argv Array of arguments + * @return Exit code + */ + + +int main(int argc, char* argv[]) { + std::cout << "ABC-EqF: Attitude-Bias-Calibration Equivariant Filter Demo" << std::endl; + std::cout << "==============================================================" << std::endl; + + try { + // Parse command line options + std::string csvFilePath; + int maxRows = -1; // Process all rows by default + int downsample = 1; // No downsampling by default + + if (argc > 1) { + csvFilePath = argv[1]; + } else { + // Try to find the EQFdata file in the GTSAM examples directory + try { + csvFilePath = findExampleDataFile("EqFdata.csv"); + } catch (const std::exception& e) { + std::cerr << "Error: Could not find EqFdata.csv" << std::endl; + std::cerr << "Usage: " << argv[0] << " [csv_file_path] [max_rows] [downsample]" << std::endl; + return 1; + } + } + + // Optional command line parameters + if (argc > 2) { + maxRows = std::stoi(argv[2]); + } + + if (argc > 3) { + downsample = std::stoi(argv[3]); + } + + // Load data from CSV file + std::vector data = loadDataFromCSV(csvFilePath, 0, maxRows, downsample); + + if (data.empty()) { + std::cerr << "No data available to process. Exiting." << std::endl; + return 1; + } + + // Initialize the EqF filter with one calibration state + int n_cal = 1; + int n_sensors = 2; + + // Initial covariance - larger values allow faster convergence + Matrix initialSigma = Matrix::Identity(6 + 3*n_cal, 6 + 3*n_cal); + initialSigma.diagonal().head<3>() = Vector3::Constant(0.1); // Attitude uncertainty + initialSigma.diagonal().segment<3>(3) = Vector3::Constant(0.01); // Bias uncertainty + initialSigma.diagonal().tail<3>() = Vector3::Constant(0.1); // Calibration uncertainty + + // Create filter + EqF filter(initialSigma, n_cal, n_sensors); + + // Process data + processDataWithEqF(filter, data); + + } catch (const std::exception& e) { + std::cerr << "Error: " << e.what() << std::endl; + return 1; + } + + std::cout << "\nEqF demonstration completed successfully." << std::endl; + return 0; +} \ No newline at end of file diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 581535c9f..8da95722c 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -17,4 +17,3 @@ if (NOT GTSAM_USE_BOOST_FEATURES) endif() gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;${Boost_PROGRAM_OPTIONS_LIBRARY}" ${GTSAM_BUILD_EXAMPLES_ALWAYS}) -add_subdirectory(ABC_EQF) \ No newline at end of file diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index a5a6f19af..7f3183aa2 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -86,7 +86,10 @@ endforeach(subdir) # To add additional sources to gtsam when building the full library (static or shared) # append the subfolder with _srcs appended to the end to this list -set(gtsam_srcs ${3rdparty_srcs}) +set(gtsam_srcs ${3rdparty_srcs} + ../examples/ABC_EQF_Demo.cpp + ../examples/ABC_EQF.cpp + ../examples/ABC_EQF.h) foreach(subdir ${gtsam_subdirs}) list(APPEND gtsam_srcs ${${subdir}_srcs}) endforeach(subdir)