ABC_EQF_Demo.cpp

Go to the documentation of this file.

 
#include "ABC_EQF.h"
 
// Use namespace for convenience
using namespace gtsam;
constexpr size_t N = 1;  // Number of calibration states
using M = abc_eqf_lib::State<N>;
using Group = abc_eqf_lib::G<N>;
using EqFilter = abc_eqf_lib::EqF<N>;
using gtsam::abc_eqf_lib::EqF;
using gtsam::abc_eqf_lib::Input;
using gtsam::abc_eqf_lib::Measurement;
 
struct Data {
  M xi;                        
  Input u;                     
  std::vector<Measurement> y;  
  int n_meas;                  
  double t;                    
  double dt;                   
};
 
//========================================================================
// Data Processing Functions
//========================================================================
 
std::vector<Data> loadDataFromCSV(const std::string& filename, int startRow = 0,
                                  int maxRows = -1, int downsample = 1);
 
void processDataWithEqF(EqFilter& filter, const std::vector<Data>& data_list,
                        int printInterval = 10);
 
//========================================================================
// Data Processing Functions Implementation
//========================================================================
 
/*
 * Loads the test data from the csv file
 * startRow First row to load based on csv, 0 by default
 * maxRows maximum rows to load, defaults to all rows
 * downsample Downsample factor, default 1
 * A list of data objects
 */
 
std::vector<Data> loadDataFromCSV(const std::string& filename, int startRow,
                                  int maxRows, int downsample) {
  std::vector<Data> 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<double> values;
 
    // Parse line into values
    while (std::getline(ss, token, ',')) {
      try {
        values.push_back(std::stod(token));
      } catch (const std::exception& e) {
        errorCount++;
        values.push_back(0.0);  // Use default value
      }
    }
 
    // Check if we have enough values
    if (values.size() < 39) {
      errorCount++;
      continue;
    }
 
    // Extract values
    double t = values[0];
    double dt = (rowCount == 0) ? 0.0 : t - prevTime;
    prevTime = t;
 
    // Create ground truth state
    Quaternion quat(values[1], values[2], values[3], values[4]);  // w, x, y, z
    Rot3 R = Rot3(quat);
 
    Vector3 b(values[5], values[6], values[7]);
 
    Quaternion calQuat(values[8], values[9], values[10],
                       values[11]);  // w, x, y, z
    std::array<Rot3, N> S = {Rot3(calQuat)};
 
    M xi(R, b, S);
 
    // Create input
    Vector3 w(values[12], values[13], values[14]);
 
    // Create input covariance matrix (6x6)
    // First 3x3 block for angular velocity, second 3x3 block for bias process
    // noise
    Matrix inputCov = Matrix::Zero(6, 6);
    inputCov(0, 0) = values[15] * values[15];  // std_w_x^2
    inputCov(1, 1) = values[16] * values[16];  // std_w_y^2
    inputCov(2, 2) = values[17] * values[17];  // std_w_z^2
    inputCov(3, 3) = values[18] * values[18];  // std_b_x^2
    inputCov(4, 4) = values[19] * values[19];  // std_b_y^2
    inputCov(5, 5) = values[20] * values[20];  // std_b_z^2
 
    Input u{w, inputCov};
 
    // Create measurements
    std::vector<Measurement> measurements;
 
    // First measurement (calibrated sensor, cal_idx = 0)
    Vector3 y0(values[21], values[22], values[23]);
    Vector3 d0(values[33], values[34], values[35]);
 
    // Normalize vectors if needed
    if (abs(y0.norm() - 1.0) > 1e-5) y0.normalize();
    if (abs(d0.norm() - 1.0) > 1e-5) d0.normalize();
 
    // Measurement covariance
    Matrix3 covY0 = Matrix3::Zero();
    covY0(0, 0) = values[27] * values[27];  // std_y_x_0^2
    covY0(1, 1) = values[28] * values[28];  // std_y_y_0^2
    covY0(2, 2) = values[29] * values[29];  // std_y_z_0^2
 
    // Create measurement
    measurements.push_back(Measurement{Unit3(y0), Unit3(d0), covY0, 0});
 
    // Second measurement (calibrated sensor, cal_idx = -1)
    Vector3 y1(values[24], values[25], values[26]);
    Vector3 d1(values[36], values[37], values[38]);
 
    // Normalize vectors if needed
    if (abs(y1.norm() - 1.0) > 1e-5) y1.normalize();
    if (abs(d1.norm() - 1.0) > 1e-5) d1.normalize();
 
    // Measurement covariance
    Matrix3 covY1 = Matrix3::Zero();
    covY1(0, 0) = values[30] * values[30];  // std_y_x_1^2
    covY1(1, 1) = values[31] * values[31];  // std_y_y_1^2
    covY1(2, 2) = values[32] * values[32];  // std_y_z_1^2
 
    // Create measurement
    measurements.push_back(Measurement{Unit3(y1), Unit3(d1), covY1, -1});
 
    // Create Data object and add to list
    data_list.push_back(Data{xi, u, measurements, 2, t, dt});
 
    rowCount++;
 
    // Show loading progress every 1000 rows
    if (rowCount % 1000 == 0) {
      std::cout << "." << std::flush;
    }
  }
 
  std::cout << " Done!" << std::endl;
  std::cout << "Loaded " << data_list.size() << " data points";
 
  if (errorCount > 0) {
    std::cout << " (" << errorCount << " errors encountered)";
  }
 
  std::cout << std::endl;
 
  return data_list;
}
 
void processDataWithEqF(EqFilter& filter, const std::vector<Data>& 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<double> att_errors;
  std::vector<double> bias_errors;
  std::vector<double> cal_errors;
 
  // Track time for performance measurement
  auto start = std::chrono::high_resolution_clock::now();
 
  int totalMeasurements = 0;
  int validMeasurements = 0;
 
  // Define constant for converting radians to degrees
  const double RAD_TO_DEG = 180.0 / M_PI;
 
  // Print a progress indicator
  int progressStep = data_list.size() / 10;  // 10 progress updates
  if (progressStep < 1) progressStep = 1;
 
  std::cout << "Progress: ";
 
  for (size_t i = 0; i < data_list.size(); i++) {
    const Data& data = data_list[i];
 
    // Propagate filter with current input and time step
    filter.propagation(data.u, data.dt);
 
    // Process all measurements
    for (const auto& y : data.y) {
      totalMeasurements++;
 
      // Skip invalid measurements
      Vector3 y_vec = y.y.unitVector();
      Vector3 d_vec = y.d.unitVector();
      if (std::isnan(y_vec[0]) || std::isnan(y_vec[1]) ||
          std::isnan(y_vec[2]) || std::isnan(d_vec[0]) ||
          std::isnan(d_vec[1]) || std::isnan(d_vec[2])) {
        continue;
      }
 
      try {
        filter.update(y);
        validMeasurements++;
      } catch (const std::exception& e) {
        std::cerr << "Error updating at t=" << data.t << ": " << e.what()
                  << std::endl;
      }
    }
 
    // Get current state estimate
    M estimate = filter.stateEstimate();
 
    // Calculate errors
    Vector3 att_error = Rot3::Logmap(data.xi.R.between(estimate.R));
    Vector3 bias_error = estimate.b - data.xi.b;
    Vector3 cal_error = Vector3::Zero();
    if (!data.xi.S.empty() && !estimate.S.empty()) {
      cal_error = Rot3::Logmap(data.xi.S[0].between(estimate.S[0]));
    }
 
    // Store errors
    att_errors.push_back(att_error.norm());
    bias_errors.push_back(bias_error.norm());
    cal_errors.push_back(cal_error.norm());
 
    // Show progress dots
    if (i % progressStep == 0) {
      std::cout << "." << std::flush;
    }
  }
 
  std::cout << " Done!" << std::endl;
 
  auto end = std::chrono::high_resolution_clock::now();
  std::chrono::duration<double> elapsed = end - start;
 
  // Calculate average errors
  double avg_att_error = 0.0;
  double avg_bias_error = 0.0;
  double avg_cal_error = 0.0;
 
  if (!att_errors.empty()) {
    avg_att_error = std::accumulate(att_errors.begin(), att_errors.end(), 0.0) /
                    att_errors.size();
    avg_bias_error =
        std::accumulate(bias_errors.begin(), bias_errors.end(), 0.0) /
        bias_errors.size();
    avg_cal_error = std::accumulate(cal_errors.begin(), cal_errors.end(), 0.0) /
                    cal_errors.size();
  }
 
  // Calculate final errors from last data point
  const Data& final_data = data_list.back();
  M final_estimate = filter.stateEstimate();
  Vector3 final_att_error =
      Rot3::Logmap(final_data.xi.R.between(final_estimate.R));
  Vector3 final_bias_error = final_estimate.b - final_data.xi.b;
  Vector3 final_cal_error = Vector3::Zero();
  if (!final_data.xi.S.empty() && !final_estimate.S.empty()) {
    final_cal_error =
        Rot3::Logmap(final_data.xi.S[0].between(final_estimate.S[0]));
  }
 
  // Print summary statistics
  std::cout << "\n=== Filter Performance Summary ===" << std::endl;
  std::cout << "Processing time: " << elapsed.count() << " seconds"
            << std::endl;
  std::cout << "Processed measurements: " << totalMeasurements
            << " (valid: " << validMeasurements << ")" << std::endl;
 
  // Average errors
  std::cout << "\n-- Average Errors --" << std::endl;
  std::cout << "Attitude: " << (avg_att_error * RAD_TO_DEG) << "°" << std::endl;
  std::cout << "Bias: " << avg_bias_error << std::endl;
  std::cout << "Calibration: " << (avg_cal_error * RAD_TO_DEG) << "°"
            << std::endl;
 
  // Final errors
  std::cout << "\n-- Final Errors --" << std::endl;
  std::cout << "Attitude: " << (final_att_error.norm() * RAD_TO_DEG) << "°"
            << std::endl;
  std::cout << "Bias: " << final_bias_error.norm() << std::endl;
  std::cout << "Calibration: " << (final_cal_error.norm() * RAD_TO_DEG) << "°"
            << std::endl;
 
  // Print a brief comparison of final estimate vs ground truth
  std::cout << "\n-- Final State vs Ground Truth --" << std::endl;
  std::cout << "Attitude (RPY) - Estimate: "
            << (final_estimate.R.rpy() * RAD_TO_DEG).transpose()
            << "° | Truth: " << (final_data.xi.R.rpy() * RAD_TO_DEG).transpose()
            << "°" << std::endl;
  std::cout << "Bias - Estimate: " << final_estimate.b.transpose()
            << " | Truth: " << final_data.xi.b.transpose() << std::endl;
 
  if (!final_estimate.S.empty() && !final_data.xi.S.empty()) {
    std::cout << "Calibration (RPY) - Estimate: "
              << (final_estimate.S[0].rpy() * RAD_TO_DEG).transpose()
              << "° | Truth: "
              << (final_data.xi.S[0].rpy() * RAD_TO_DEG).transpose() << "°"
              << std::endl;
  }
}
 
int main(int argc, char* argv[]) {
  std::cout << "ABC-EqF: Attitude-Bias-Calibration Equivariant Filter Demo"
            << std::endl;
  std::cout << "=============================================================="
            << std::endl;
 
  try {
    // Parse command line options
    std::string csvFilePath;
    int maxRows = -1;    // Process all rows by default
    int downsample = 1;  // No downsampling by default
 
    if (argc > 1) {
      csvFilePath = argv[1];
    } else {
      // Try to find the EQFdata file in the GTSAM examples directory
      try {
        csvFilePath = findExampleDataFile("EqFdata.csv");
      } catch (const std::exception& e) {
        std::cerr << "Error: Could not find EqFdata.csv" << std::endl;
        std::cerr << "Usage: " << argv[0]
                  << " [csv_file_path] [max_rows] [downsample]" << std::endl;
        return 1;
      }
    }
 
    // Optional command line parameters
    if (argc > 2) {
      maxRows = std::stoi(argv[2]);
    }
 
    if (argc > 3) {
      downsample = std::stoi(argv[3]);
    }
 
    // Load data from CSV file
    std::vector<Data> data =
        loadDataFromCSV(csvFilePath, 0, maxRows, downsample);
 
    if (data.empty()) {
      std::cerr << "No data available to process. Exiting." << std::endl;
      return 1;
    }
 
    // Initialize the EqF filter with one calibration state
    int n_sensors = 2;
 
    // Initial covariance - larger values allow faster convergence
    Matrix initialSigma = Matrix::Identity(6 + 3 * N, 6 + 3 * N);
    initialSigma.diagonal().head<3>() =
        Vector3::Constant(0.1);  // Attitude uncertainty
    initialSigma.diagonal().segment<3>(3) =
        Vector3::Constant(0.01);  // Bias uncertainty
    initialSigma.diagonal().tail<3>() =
        Vector3::Constant(0.1);  // Calibration uncertainty
 
    // Create filter
    EqFilter filter(initialSigma, n_sensors);
 
    // Process data
    processDataWithEqF(filter, data);
 
  } catch (const std::exception& e) {
    std::cerr << "Error: " << e.what() << std::endl;
    return 1;
  }
 
  std::cout << "\nEqF demonstration completed successfully." << std::endl;
  return 0;
}