c++ implementation - use with caution

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)

examples/ABC_EQF/Direction.h

@@ -5,23 +5,31 @@
 #ifndef DIRECTION_H
 #define DIRECTION_H
 //#pragma once
-
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/base/Vector.h>
-
 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

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) {
+  if (y.cal_idx > __n_cal) {
-        throw std::invalid_argument("Calibration index out of range");
+    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 Ct = __measurementMatrixC(y.d, y.cal_idx);
-    Matrix S = Ct * __Sigma * Ct.transpose() + Dt * y.Sigma * Dt.transpose();
+  Vector3 action_result = outputAction(__X_hat.inv(), y.y, y.cal_idx);
-    Matrix K = __Sigma * Ct.transpose() * S.inverse();
+  Vector3 delta_vec = wedge(y.d.d.unitVector()) * action_result;  // Ensure this is the right operation
-
+  Matrix Dt = __outputMatrixDt(y.cal_idx);
-    Vector Delta = __InnovationLift * K * delta_vec;
+  Matrix S = Ct * __Sigma * Ct.transpose() + Dt * y.Sigma * Dt.transpose();
-    __X_hat = G::exp(Delta) * __X_hat;
+  Matrix K = __Sigma * Ct.transpose() * S.inverse();
-    __Sigma = (Matrix::Identity(__dof, __dof) - K * Ct) * __Sigma;
+  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 the measurement is related to a sensor that has a calibration state
-    if (idx >= 0) {
+  if (idx >= 0) {
-        Cc.block<3, 3>(0, 3 * idx) = wedge(d.d.unitVector());
+    // 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());
+  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;
+  // 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 {

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 <iostream>
 #include <fstream>
@@ -158,38 +159,65 @@ void runSimulation(EqF& filter, const std::vector<Data>& 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> 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
+    std::string csvFilePath;
-    int n_cal = 1;      // Number of calibration states
+    
-    int n_sensors = 2;  // Number of sensors
+    // Check if CSV file path is provided as command line argument
-
+    if (argc > 1) {
-    // Initial covariance - larger values for higher uncertainty
+        csvFilePath = argv[1];
-    Matrix initialSigma = Matrix::Identity(6 + 3*n_cal, 6 + 3*n_cal);
+    } else {
-    initialSigma.diagonal().head<3>() = Vector3::Constant(0.5); // Attitude uncertainty
+        std::cout << "Please enter the path to your CSV data file: ";
-    initialSigma.diagonal().segment<3>(3) = Vector3::Constant(0.1); // Bias uncertainty
+        std::cin >> csvFilePath;
-    initialSigma.diagonal().tail<3>() = Vector3::Constant(0.5); // Calibration uncertainty
+    }
-
+    
-    std::cout << "Creating filter with " << n_cal << " calibration states..." << std::endl;
+    std::cout << "Using CSV data from: " << csvFilePath << std::endl;
-
+    
     try {
-        // Create filter
+        // Run with CSV data
-        EqF filter(initialSigma, n_cal, n_sensors);
+        runEqFWithCSVData(csvFilePath);
-
-        // Generate simulated data
-        std::cout << "Generating simulated data..." << std::endl;
-        std::vector<Data> 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;
+
 }