diff --git a/examples/ABC_EQF/CMakeLists.txt b/examples/ABC_EQF/CMakeLists.txt new file mode 100644 index 000000000..f1be48997 --- /dev/null +++ b/examples/ABC_EQF/CMakeLists.txt @@ -0,0 +1,13 @@ +add_executable(ABC_EqF + main.cpp + EqF.cpp + State.cpp + Input.cpp + G.cpp + Direction.cpp + Measurements.cpp + utilities.cpp +) + +target_link_libraries(ABC_EqF gtsam) + diff --git a/examples/ABC_EQF/Data.h b/examples/ABC_EQF/Data.h new file mode 100644 index 000000000..2e6005572 --- /dev/null +++ b/examples/ABC_EQF/Data.h @@ -0,0 +1,41 @@ +// +// 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 new file mode 100644 index 000000000..751324b0b --- /dev/null +++ b/examples/ABC_EQF/Direction.cpp @@ -0,0 +1,13 @@ +// +// 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 new file mode 100644 index 000000000..633dd122d --- /dev/null +++ b/examples/ABC_EQF/Direction.h @@ -0,0 +1,27 @@ +// +// 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); +}; +#endif //DIRECTION_H diff --git a/examples/ABC_EQF/EqF.cpp b/examples/ABC_EQF/EqF.cpp new file mode 100644 index 000000000..b53eedf32 --- /dev/null +++ b/examples/ABC_EQF/EqF.cpp @@ -0,0 +1,258 @@ +// +// 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 - vee(X.a)), + new_S); +} + +Input velocityAction(const G& X, const Input& u) { + return Input(X.A.inverse().matrix() * (u.w - 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; +} + +void EqF::update(const Measurement& y) { + if (y.cal_idx > __n_cal) { + throw std::invalid_argument("Calibration index out of range"); + } + + Matrix Ct = __measurementMatrixC(y.d, y.cal_idx); + + Vector3 action_result = outputAction(__X_hat.inv(), y.y, y.cal_idx); + Vector3 delta_vec = wedge(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; +} + +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()); + } + + Matrix3 wedge_d = wedge(d.d.unitVector()); + Matrix result(3, __dof); + result.block<3, 3>(0, 0) = wedge_d; + result.block<3, 3>(0, 3) = Matrix3::Zero(); + result.block(0, 6, 3, 3 * __n_cal) = Cc; + + return result; +} + +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 new file mode 100644 index 000000000..d09099ac1 --- /dev/null +++ b/examples/ABC_EQF/EqF.h @@ -0,0 +1,153 @@ +// +// 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 new file mode 100644 index 000000000..1fb765c4f --- /dev/null +++ b/examples/ABC_EQF/G.cpp @@ -0,0 +1,61 @@ +// +// 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 + wedge(A.matrix() * 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(), + -wedge(A_inv * 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 = Rot3LeftJacobian(x.head<3>()) * x.segment<3>(3); + Matrix3 a = wedge(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 new file mode 100644 index 000000000..397303ac4 --- /dev/null +++ b/examples/ABC_EQF/G.h @@ -0,0 +1,63 @@ +// +// 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 new file mode 100644 index 000000000..4e55a4ed0 --- /dev/null +++ b/examples/ABC_EQF/Input.cpp @@ -0,0 +1,29 @@ +// +// Created by darshan on 3/11/25. +// +#include "Input.h" +#include "utilities.h" +#include +#include + +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 wedge(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 new file mode 100644 index 000000000..437d63b14 --- /dev/null +++ b/examples/ABC_EQF/Input.h @@ -0,0 +1,41 @@ +// +// 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 new file mode 100644 index 000000000..f1bd987ab --- /dev/null +++ b/examples/ABC_EQF/Measurements.cpp @@ -0,0 +1,17 @@ +// +// 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 new file mode 100644 index 000000000..b74cc538c --- /dev/null +++ b/examples/ABC_EQF/Measurements.h @@ -0,0 +1,36 @@ +// +// 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 new file mode 100644 index 000000000..90dabfbda --- /dev/null +++ b/examples/ABC_EQF/State.cpp @@ -0,0 +1,12 @@ +// +// 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 new file mode 100644 index 000000000..6c8b9c5a6 --- /dev/null +++ b/examples/ABC_EQF/State.h @@ -0,0 +1,29 @@ +// +// 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 new file mode 100644 index 000000000..540bbfc01 --- /dev/null +++ b/examples/ABC_EQF/main.cpp @@ -0,0 +1,195 @@ +// +// 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 "utilities.h" +#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; + + // Initialize filter + int n_cal = 1; // Number of calibration states + int n_sensors = 2; // Number of sensors + + // Initial covariance - larger values for higher uncertainty + Matrix initialSigma = Matrix::Identity(6 + 3*n_cal, 6 + 3*n_cal); + initialSigma.diagonal().head<3>() = Vector3::Constant(0.5); // Attitude uncertainty + initialSigma.diagonal().segment<3>(3) = Vector3::Constant(0.1); // Bias uncertainty + initialSigma.diagonal().tail<3>() = Vector3::Constant(0.5); // Calibration uncertainty + + std::cout << "Creating filter with " << n_cal << " calibration states..." << std::endl; + + try { + // Create filter + EqF filter(initialSigma, n_cal, n_sensors); + + // Generate simulated data + std::cout << "Generating simulated data..." << std::endl; + std::vector data = loadSimulatedData(); + + // Run simulation + runSimulation(filter, data); + + } 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/utilities.cpp b/examples/ABC_EQF/utilities.cpp new file mode 100644 index 000000000..74f4a97c9 --- /dev/null +++ b/examples/ABC_EQF/utilities.cpp @@ -0,0 +1,87 @@ +// +// Created by darshan on 3/11/25. +// +#include "utilities.h" +#include + +// Global configuration +const std::string COORDINATE = "EXPONENTIAL"; // Alternative: "NORMAL" + +Matrix3 wedge(const Vector3& vec) { + Matrix3 mat; + mat << 0.0, -vec(2), vec(1), + vec(2), 0.0, -vec(0), + -vec(1), vec(0), 0.0; + return mat; +} + +Vector3 vee(const Matrix3& mat) { + Vector3 vec; + vec << mat(2, 1), mat(0, 2), mat(1, 0); + return vec; +} + +Matrix3 Rot3LeftJacobian(const Vector3& arr) { + double angle = arr.norm(); + + // Near |phi|==0, use first order Taylor expansion + if (angle < 1e-10) { + return Matrix3::Identity() + 0.5 * wedge(arr); + } + + Vector3 axis = arr / angle; + double s = sin(angle); + double c = cos(angle); + + return (s / angle) * Matrix3::Identity() + + (1 - (s / angle)) * (axis * axis.transpose()) + + ((1 - c) / angle) * wedge(axis); +} + +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 new file mode 100644 index 000000000..f19b06c07 --- /dev/null +++ b/examples/ABC_EQF/utilities.h @@ -0,0 +1,30 @@ +// +// 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); +Vector3 vee(const Matrix3& mat); +Matrix3 Rot3LeftJacobian(const Vector3& arr); +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/CMakeLists.txt b/examples/CMakeLists.txt index 8da95722c..581535c9f 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -17,3 +17,4 @@ 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