From d9cd90589c476cad53ee36aad02514d075282d00 Mon Sep 17 00:00:00 2001 From: darshan-17 Date: Wed, 19 Mar 2025 21:04:32 -0700 Subject: [PATCH] c++ implementation - use with caution --- examples/ABC_EQF/CMakeLists.txt | 1 + examples/ABC_EQF/Direction.h | 16 +++++-- examples/ABC_EQF/EqF.cpp | 73 +++++++++++++++++++++---------- examples/ABC_EQF/main.cpp | 76 ++++++++++++++++++++++----------- 4 files changed, 115 insertions(+), 51 deletions(-) diff --git a/examples/ABC_EQF/CMakeLists.txt b/examples/ABC_EQF/CMakeLists.txt index f1be48997..23eb309d2 100644 --- a/examples/ABC_EQF/CMakeLists.txt +++ b/examples/ABC_EQF/CMakeLists.txt @@ -7,6 +7,7 @@ add_executable(ABC_EqF Direction.cpp Measurements.cpp utilities.cpp + runEQF_withcsv.h ) target_link_libraries(ABC_EqF gtsam) diff --git a/examples/ABC_EQF/Direction.h b/examples/ABC_EQF/Direction.h index 633dd122d..041b90bb6 100644 --- a/examples/ABC_EQF/Direction.h +++ b/examples/ABC_EQF/Direction.h @@ -5,23 +5,31 @@ #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 index b53eedf32..418d9ac08 100644 --- a/examples/ABC_EQF/EqF.cpp +++ b/examples/ABC_EQF/EqF.cpp @@ -152,23 +152,43 @@ void EqF::propagation(const Input& u, double 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"); + 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 = 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 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; // 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 { @@ -229,20 +249,27 @@ Matrix EqF::__inputMatrixBt() const { } Matrix EqF::__measurementMatrixC(const Direction& d, int idx) const { - Matrix Cc = Matrix::Zero(3, 3 * __n_cal); + 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()); - } + // 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) = 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; + Matrix3 wedge_d = wedge(d.d.unitVector()); - return result; + // 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 { diff --git a/examples/ABC_EQF/main.cpp b/examples/ABC_EQF/main.cpp index 540bbfc01..c11d3a179 100644 --- a/examples/ABC_EQF/main.cpp +++ b/examples/ABC_EQF/main.cpp @@ -7,6 +7,7 @@ #include "Direction.h" #include "Measurements.h" #include "Data.h" +#include "runEQF_withcsv.h" #include "utilities.h" #include #include @@ -158,38 +159,65 @@ void runSimulation(EqF& filter, const std::vector& data) { 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; 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; - + + std::string csvFilePath; + + // Check if CSV file path is provided as command line argument + if (argc > 1) { + csvFilePath = argv[1]; + } else { + std::cout << "Please enter the path to your CSV data file: "; + std::cin >> csvFilePath; + } + + std::cout << "Using CSV data from: " << csvFilePath << 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); - + // 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