12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

c++ implementation - use with caution

release/4.3a0
darshan-17 2025-03-19 21:04:32 -07:00 committed by jenniferoum
parent 673336545b
commit d9cd90589c
4 changed files with 115 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
examples/ABC_EQF/CMakeLists.txt
View File

@ -7,6 +7,7 @@ add_executable(ABC_EqF
Direction.cpp
Measurements.cpp
utilities.cpp
runEQF_withcsv.h
)
target_link_libraries(ABC_EqF gtsam)

16
examples/ABC_EQF/Direction.h
View File

@ -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

73
examples/ABC_EQF/EqF.cpp
View File

@ -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 {

76
examples/ABC_EQF/main.cpp
View File

@ -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
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> 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;
}