diff --git a/examples/AbcEquivariantFilterExample.cpp b/examples/AbcEquivariantFilterExample.cpp index 821db414ef..7360091bcd 100644 --- a/examples/AbcEquivariantFilterExample.cpp +++ b/examples/AbcEquivariantFilterExample.cpp @@ -1,32 +1,31 @@ /** * @file AbcEquivariantFilterExample.cpp - * @brief Demonstration of the full Attitude-Bias-Calibration Equivariant Filter + * @brief Demonstration of the Attitude-Bias-Calibration Equivariant Filter * * This demo shows the Equivariant Filter (EqF) for attitude estimation * with both gyroscope bias and sensor extrinsic calibration, based on the * paper: "Overcoming Bias: Equivariant Filter Design for Biased Attitude * Estimation with Online Calibration" by Fornasier et al. * + * This example uses the simplified AbcEquivariantFilter class which + * provides a clean interface for predict/update operations without requiring + * manual computation of Jacobian matrices and innovation functions. + * * @author Darshan Rajasekaran * @author Jennifer Oum * @author Rohan Bansal * @author Frank Dellaert * @date 2025 */ -#include #include #include +#include // Use namespace for convenience using namespace gtsam; constexpr size_t n = 1; // Number of calibration states using M = abc::State; -using G = abc::Group; -using Symmetry = abc::Symmetry; -using EqFilter = gtsam::EquivariantFilter; -using Lift = abc::Lift; -using InputOrbit = typename abc::InputAction::Orbit; -using Innovation = abc::Innovation; +using AbcFilter = abc::AbcEquivariantFilter; /// Measurement struct struct Measurement { @@ -72,7 +71,7 @@ std::vector loadDataFromCSV(const std::string& filename, int startRow = 0, int maxRows = -1, int downsample = 1); /// Process data with EqF and print summary results -void processDataWithEqF(EqFilter& filter, const std::vector& data_list, +void processDataWithEqF(AbcFilter& filter, const std::vector& data_list, int printInterval = 10); //======================================================================== @@ -235,7 +234,7 @@ std::vector loadDataFromCSV(const std::string& filename, int startRow, } /// Takes in the data and runs an EqF on it and reports the results -void processDataWithEqF(EqFilter& filter, const std::vector& data_list, +void processDataWithEqF(AbcFilter& filter, const std::vector& data_list, int printInterval) { if (data_list.empty()) { std::cerr << "No data to process" << std::endl; @@ -267,18 +266,7 @@ void processDataWithEqF(EqFilter& filter, const std::vector& data_list, for (size_t i = 0; i < data_list.size(); i++) { const Data& data = data_list[i]; - Matrix Q = abc::inputProcessNoise(data.inputCovariance); - // Propagate filter with current input and time step - Vector6 u = abc::toInputVector(data.omega); - Lift lift_u(u); - InputOrbit psi_u(u); - - // Use Explicit Matrices API - G X_hat = filter.groupEstimate(); - Matrix A = abc::stateMatrixA(psi_u, X_hat); - Matrix B = abc::inputMatrixB(X_hat); - Matrix Qc = B * Q * B.transpose(); // continuous-time manifold covariance - filter.predictWithJacobian<2>(lift_u, A, Qc, data.dt); + filter.predict(data.omega, data.inputCovariance, data.dt); // Process all measurements for (const auto& measurement : data.measurements) { @@ -294,14 +282,8 @@ void processDataWithEqF(EqFilter& filter, const std::vector& data_list, } try { - Innovation innovation(measurement.y, measurement.d, - measurement.cal_idx); - // Use Explicit Matrices API - X_hat = filter.groupEstimate(); - const Matrix3 D = abc::outputMatrixD(X_hat, measurement.cal_idx); - const Matrix3 R = D * measurement.R * D.transpose(); - filter.update(innovation, Z_3x1, R); - + filter.update(measurement.y, measurement.d, measurement.R, + measurement.cal_idx); validMeasurements++; } catch (const std::exception& e) { std::cerr << "Error updating at t=" << data.t << ": " << e.what() @@ -309,15 +291,17 @@ void processDataWithEqF(EqFilter& filter, const std::vector& data_list, } } - // Get current state estimate - M estimate = filter.state(); + // Get current estimates using accessor methods + Rot3 att_est = filter.attitude(); + Vector3 bias_est = filter.bias(); + Rot3 cal_est = filter.calibration(0); // Calculate errors - Vector3 att_error = Rot3::Logmap(data.xi.R.between(estimate.R)); - Vector3 bias_error = estimate.b - data.xi.b; + Vector3 att_error = Rot3::Logmap(data.xi.R.between(att_est)); + Vector3 bias_error = bias_est - data.xi.b; Vector3 cal_error = Z_3x1; - if (!data.xi.S.empty() && !estimate.S.empty()) { - cal_error = Rot3::Logmap(data.xi.S[0].between(estimate.S[0])); + if (!data.xi.S.empty()) { + cal_error = Rot3::Logmap(data.xi.S[0].between(cal_est)); } // Store errors @@ -353,14 +337,16 @@ void processDataWithEqF(EqFilter& filter, const std::vector& data_list, // Calculate final errors from last data point const Data& final_data = data_list.back(); - M final_estimate = filter.state(); + Rot3 final_att_est = filter.attitude(); + Vector3 final_bias_est = filter.bias(); + Rot3 final_cal_est = filter.calibration(0); + 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; + Rot3::Logmap(final_data.xi.R.between(final_att_est)); + Vector3 final_bias_error = final_bias_est - final_data.xi.b; Vector3 final_cal_error = Z_3x1; - 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])); + if (!final_data.xi.S.empty()) { + final_cal_error = Rot3::Logmap(final_data.xi.S[0].between(final_cal_est)); } // Print summary statistics @@ -388,16 +374,15 @@ void processDataWithEqF(EqFilter& filter, const std::vector& data_list, // 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() + << (final_att_est.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() + std::cout << "Bias - Estimate: " << final_bias_est.transpose() << " | Truth: " << final_data.xi.b.transpose() << std::endl; - if (!final_estimate.S.empty() && !final_data.xi.S.empty()) { + if (!final_data.xi.S.empty()) { std::cout << "Calibration (RPY) - Estimate: " - << (final_estimate.S[0].rpy() * RAD_TO_DEG).transpose() - << "° | Truth: " + << (final_cal_est.rpy() * RAD_TO_DEG).transpose() << "° | Truth: " << (final_data.xi.S[0].rpy() * RAD_TO_DEG).transpose() << "°" << std::endl; } @@ -456,10 +441,8 @@ int main(int argc, char* argv[]) { initialSigma.diagonal().tail<3>() = Vector3::Constant(0.1); // Calibration uncertainty - M initialState = M::identity(); - - // Create filter - EqFilter filter(initialState, initialSigma); + // Create filter with initial covariance (starts at identity state) + AbcFilter filter(initialSigma); // Process data processDataWithEqF(filter, data); diff --git a/gtsam/navigation/EquivariantFilter.h b/gtsam/navigation/EquivariantFilter.h index 501ac9f2d3..187f0d3cb1 100644 --- a/gtsam/navigation/EquivariantFilter.h +++ b/gtsam/navigation/EquivariantFilter.h @@ -93,7 +93,7 @@ class EquivariantFilter : public ManifoldEKF { using Base::state; /// errorCovariance that returns P_, on the equivariant filter error - Matrix errorCovariance() const { return this->P_; } + const typename Base::Covariance& errorCovariance() const { return this->P_; } /// Covariance in the tangent space at the current state. CovarianceM covariance() const { diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 5c26698e1f..39a3077e8e 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -651,6 +651,27 @@ virtual class InvariantEKF : gtsam::LeftLinearEKF { void predict(const gtsam::Vector& u, double dt, gtsam::Matrix Q); }; +// --------------------------------------------------------------------------- +// ABC Equivariant Filter +#include +namespace abc { +template +class AbcEquivariantFilter { + // Constructors + AbcEquivariantFilter(); + AbcEquivariantFilter(gtsam::Matrix Sigma0); + + // Predict and update methods + void predict(const gtsam::Vector3& omega, const gtsam::Matrix6& inputCovariance, double dt); + void update(const gtsam::Unit3& y, const gtsam::Unit3& d, const gtsam::Matrix3& R, int cal_idx); + + // Accessors + gtsam::Rot3 attitude() const; + gtsam::Vector3 bias() const; + gtsam::Rot3 calibration(size_t i) const; +}; +} // namespace abc + // Specialized NavState IMU EKF #include class NavStateImuEKF : gtsam::LeftLinearEKF { diff --git a/gtsam_unstable/geometry/ABCEquivariantFilter.h b/gtsam_unstable/geometry/ABCEquivariantFilter.h new file mode 100644 index 0000000000..fb6a7695e0 --- /dev/null +++ b/gtsam_unstable/geometry/ABCEquivariantFilter.h @@ -0,0 +1,136 @@ +/** + * @file ABCEquivariantFilter.h + * @brief Attitude-Bias-Calibration Equivariant Filter for state estimation + * @author Rohan Bansal + * @date 2026 + * + * This file extends the EquivariantFilter class to provide a more user-friendly + * interface for the ABC Equivariant Filter. This class templates on the number + * of calibrated sensors N, abstracting away the details of the ABC system. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam { +namespace abc { + +/** + * @class AbcEquivariantFilter + * @brief Equivariant Filter for Attitude-Bias-Calibration (ABC) estimation + * + * This class implements an equivariant filter for estimating: + * - Attitude (rotation): The orientation of the body frame relative to a + * reference frame + * - Bias: Gyroscope bias correction vector (3D) + * - Calibration: N sensor calibration rotation matrices + * + * The filter uses the ABC Lie group structure and equivariant dynamics to + * provide consistent state estimation. It inherits from EquivariantFilter + * and provides a simplified interface for ABC-specific operations. + * + * @tparam N Number of calibrated sensors (typically 1 or more) + */ +template +class AbcEquivariantFilter + : public gtsam::EquivariantFilter, Symmetry> { + public: + using gtsam::EquivariantFilter, Symmetry>::update; + + /** + * @brief Default constructor with identity initial covariance + * + * Initializes the filter with an identity state and identity covariance + * matrix of dimension (6 + 3*N) x (6 + 3*N), where 6 accounts for the + * attitude and bias parameters, and 3*N for N calibration parameters. + */ + AbcEquivariantFilter() + : AbcEquivariantFilter(Matrix::Identity(6 + 3 * N, 6 + 3 * N)) {} + + /** + * @brief Construct filter with custom initial covariance + * + * Initializes the filter at the identity state with a specified initial + * covariance matrix on the manifold tangent space. + * + * @param Sigma0 Initial covariance matrix (6+3*N) x (6+3*N) + */ + explicit AbcEquivariantFilter(const Matrix& Sigma0) + : gtsam::EquivariantFilter, Symmetry>(State::identity(), + Sigma0) {} + + /** + * @brief Prediction step using gyroscope measurements + * + * Propagates the filter state forward in time using angular velocity + * measurements and process noise. This method uses the explicit Jacobian + * matrices (A and B) computed from the ABC dynamics. + * + * @param omega Angular velocity measurement (body frame, rad/s) + * @param inputCovariance Process noise covariance (6x6) for the input + * @param dt Time step (seconds) + */ + void predict(const Vector3& omega, const Matrix6& inputCovariance, + double dt) { + const Matrix Q = inputProcessNoise(inputCovariance); + const Vector6 u = toInputVector(omega); + const Lift lift_u(u); + const typename InputAction::Orbit psi_u(u); + + const Group X_hat = this->groupEstimate(); + const Matrix A = stateMatrixA(psi_u, X_hat); + const Matrix B = inputMatrixB(X_hat); + const Matrix Qc = B * Q * B.transpose(); + + this->template predictWithJacobian<2>(lift_u, A, Qc, dt); + } + + /** + * @brief Measurement update using a direction observation + * + * Corrects the filter estimate using a direction measurement from a + * calibrated sensor. The measurement model assumes the sensor observes + * a known reference direction in the body frame. + * + * @param y Measured direction (unit vector in body frame) + * @param d Reference direction (unit vector in reference frame) + * @param R Measurement noise covariance (3x3) + * @param cal_idx Index of the calibrated sensor (0 to N-1), or -1 for + * uncalibrated measurements + */ + void update(const Unit3& y, const Unit3& d, const Matrix3& R, int cal_idx) { + const Innovation innovation(y, d, cal_idx); + const Group X_hat = this->groupEstimate(); + const Matrix3 D = outputMatrixD(X_hat, cal_idx); + const Matrix3 R_adjusted = D * R * D.transpose(); + this->template update(innovation, Z_3x1, R_adjusted); + } + + /** + * @brief Get current attitude estimate + * @return Current rotation estimate (body frame to reference frame) + */ + Rot3 attitude() const { return this->state().R; } + + /** + * @brief Get current gyroscope bias estimate + * @return Current bias vector (rad/s) + */ + Vector3 bias() const { return this->state().b; } + + /** + * @brief Get calibration estimate for a specific sensor + * @param i Sensor index (0 to N-1) + * @return Calibration rotation for sensor i + */ + Rot3 calibration(size_t i) const { return this->state().S[i]; } +}; + +} // namespace abc +} // namespace gtsam diff --git a/python/gtsam/examples/AbcEquivariantFilterExample.py b/python/gtsam/examples/AbcEquivariantFilterExample.py new file mode 100644 index 0000000000..4e1879900d --- /dev/null +++ b/python/gtsam/examples/AbcEquivariantFilterExample.py @@ -0,0 +1,250 @@ +""" +Python translation of examples/AbcEquivariantFilterExample.cpp. + +Runs the Attitude-Bias-Calibration EqF demo using the wrapped C++ +EquivariantFilter (ABC-specific wrapper). +""" + +from __future__ import annotations + +from dataclasses import dataclass +import csv +import math +from typing import List + +import numpy as np +import gtsam +from gtsam import Rot3, Unit3 +from gtsam.utils import findExampleDataFile + + +@dataclass +class MeasurementRecord: + y: np.ndarray + d: np.ndarray + R: np.ndarray + cal_idx: int + + +@dataclass +class DataRecord: + R: Rot3 + b: np.ndarray + cal_rot: Rot3 + omega: np.ndarray + input_covariance: np.ndarray + measurements: List[MeasurementRecord] + t: float + dt: float + + +def _normalize(v: np.ndarray) -> np.ndarray: + n = np.linalg.norm(v) + if n == 0.0: + return v + return v / n + + +def load_data_from_csv( + filename: str, start_row: int = 0, max_rows: int = -1, downsample: int = 1 +) -> List[DataRecord]: + data_list: List[DataRecord] = [] + with open(filename, newline="") as csvfile: + reader = csv.reader(csvfile) + header = next(reader, None) + if header is None: + return data_list + + line_number = 1 + row_count = 0 + prev_time = 0.0 + + for row in reader: + line_number += 1 + if line_number < start_row: + continue + if ((line_number - start_row - 1) % downsample) != 0: + continue + if max_rows != -1 and row_count >= max_rows: + break + if len(row) < 39: + continue + + values = [float(x) if x else 0.0 for x in row] + + t = values[0] + dt = 0.0 if row_count == 0 else t - prev_time + prev_time = t + + R = Rot3.Quaternion(values[1], values[2], values[3], values[4]) + b = np.array([values[5], values[6], values[7]]) + + cal_rot = Rot3.Quaternion(values[8], values[9], values[10], values[11]) + + omega = np.array([values[12], values[13], values[14]]) + + input_covariance = np.zeros((6, 6)) + input_covariance[0, 0] = values[15] ** 2 + input_covariance[1, 1] = values[16] ** 2 + input_covariance[2, 2] = values[17] ** 2 + input_covariance[3, 3] = values[18] ** 2 + input_covariance[4, 4] = values[19] ** 2 + input_covariance[5, 5] = values[20] ** 2 + + measurements: List[MeasurementRecord] = [] + + y0 = _normalize(np.array([values[21], values[22], values[23]])) + d0 = _normalize(np.array([values[33], values[34], values[35]])) + cov_y0 = np.diag([values[27] ** 2, values[28] ** 2, values[29] ** 2]) + measurements.append(MeasurementRecord(y0, d0, cov_y0, 0)) + + y1 = _normalize(np.array([values[24], values[25], values[26]])) + d1 = _normalize(np.array([values[36], values[37], values[38]])) + cov_y1 = np.diag([values[30] ** 2, values[31] ** 2, values[32] ** 2]) + measurements.append(MeasurementRecord(y1, d1, cov_y1, -1)) + + data_list.append( + DataRecord( + R=R, + b=b, + cal_rot=cal_rot, + omega=omega, + input_covariance=input_covariance, + measurements=measurements, + t=t, + dt=dt, + ) + ) + row_count += 1 + + return data_list + + +def process_data_with_eqf( + filter_eqf: gtsam.abc.AbcEquivariantFilter1, + data_list: List[DataRecord], +) -> None: + if not data_list: + print("No data to process") + return + + print(f"Processing {len(data_list)} data points with EqF...") + att_errors: List[float] = [] + bias_errors: List[float] = [] + cal_errors: List[float] = [] + + total_measurements = 0 + valid_measurements = 0 + + rad_to_deg = 180.0 / math.pi + progress_step = max(len(data_list) // 10, 1) + print("Progress: ", end="", flush=True) + + for i, data in enumerate(data_list): + filter_eqf.predict(data.omega, data.input_covariance, data.dt) + + for measurement in data.measurements: + total_measurements += 1 + if np.any(np.isnan(measurement.y)) or np.any(np.isnan(measurement.d)): + continue + try: + y_unit = Unit3(measurement.y) + d_unit = Unit3(measurement.d) + filter_eqf.update(y_unit, d_unit, measurement.R, measurement.cal_idx) + valid_measurements += 1 + except Exception: + continue + + estimate_R = filter_eqf.attitude() + estimate_b = np.array(filter_eqf.bias()).reshape(3) + estimate_cal = filter_eqf.calibration(0) + + att_error = Rot3.Logmap(data.R.between(estimate_R)) + bias_error = estimate_b - data.b + cal_error = np.zeros(3) + cal_error = Rot3.Logmap(data.cal_rot.between(estimate_cal)) + + att_errors.append(np.linalg.norm(att_error)) + bias_errors.append(np.linalg.norm(bias_error)) + cal_errors.append(np.linalg.norm(cal_error)) + + if i % progress_step == 0: + print(".", end="", flush=True) + + print(" Done!") + + avg_att_error = float(np.mean(att_errors)) if att_errors else 0.0 + avg_bias_error = float(np.mean(bias_errors)) if bias_errors else 0.0 + avg_cal_error = float(np.mean(cal_errors)) if cal_errors else 0.0 + + final_data = data_list[-1] + final_R = filter_eqf.attitude() + final_b = np.array(filter_eqf.bias()).reshape(3) + final_cal = filter_eqf.calibration(0) + final_att_error = Rot3.Logmap(final_data.R.between(final_R)) + final_bias_error = final_b - final_data.b + final_cal_error = Rot3.Logmap(final_data.cal_rot.between(final_cal)) + + print("\n=== Filter Performance Summary ===") + print(f"Processed measurements: {total_measurements} (valid: {valid_measurements})") + + print("\n-- Average Errors --") + print(f"Attitude: {avg_att_error * rad_to_deg}°") + print(f"Bias: {avg_bias_error}") + print(f"Calibration: {avg_cal_error * rad_to_deg}°") + + print("\n-- Final Errors --") + print(f"Attitude: {np.linalg.norm(final_att_error) * rad_to_deg}°") + print(f"Bias: {np.linalg.norm(final_bias_error)}") + print(f"Calibration: {np.linalg.norm(final_cal_error) * rad_to_deg}°") + + print("\n-- Final State vs Ground Truth --") + print( + "Attitude (RPY) - Estimate:", + (final_R.rpy() * rad_to_deg), + "° | Truth:", + (final_data.R.rpy() * rad_to_deg), + "°", + ) + print("Bias - Estimate:", final_b, "| Truth:", final_data.b) + print( + "Calibration (RPY) - Estimate:", + (final_cal.rpy() * rad_to_deg), + "° | Truth:", + (final_data.cal_rot.rpy() * rad_to_deg), + "°", + ) + + +def main() -> None: + print("ABC-EqF: Attitude-Bias-Calibration Equivariant Filter Demo") + print("==============================================================") + + try: + csv_file_path = findExampleDataFile("EqFdata.csv") + except Exception: + print("Error: Could not find EqFdata.csv") + return + + data = load_data_from_csv(csv_file_path) + if not data: + print("No data available to process. Exiting.") + return + + n_cal = 1 + m_sensors = 2 + initial_sigma = np.eye(6 + 3 * n_cal) + initial_sigma[0:3, 0:3] = 0.1 * np.eye(3) + initial_sigma[3:6, 3:6] = 0.01 * np.eye(3) + initial_sigma[6:9, 6:9] = 0.1 * np.eye(3) + + filter_eqf = gtsam.abc.AbcEquivariantFilter1(initial_sigma) + process_data_with_eqf(filter_eqf, data) + + print("\nEqF demonstration completed successfully.") + + +if __name__ == "__main__": + main() + +