Template on size_t, Darshan's updates to cleanup comments, default coordinate to exponential, separate filter and demo specific functions, rename stateAction to operator *, fix brace initialization
jenniferoum
parent
51e20eca58
commit
17bf752576
|
|
@ -0,0 +1,259 @@
|
|||
/**
|
||||
* @file ABC.h
|
||||
* @brief Core components for Attitude-Bias-Calibration systems
|
||||
*
|
||||
* This file contains fundamental components and utilities for the ABC system
|
||||
* based on the paper "Overcoming Bias: Equivariant Filter Design for Biased
|
||||
* Attitude Estimation with Online Calibration" by Fornasier et al.
|
||||
* Authors: Darshan Rajasekaran & Jennifer Oum
|
||||
*/
|
||||
#ifndef ABC_H
|
||||
#define ABC_H
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
|
||||
namespace gtsam {
|
||||
namespace abc_eqf_lib {
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
//========================================================================
|
||||
// Utility Functions
|
||||
//========================================================================
|
||||
|
||||
//========================================================================
|
||||
// Utility Functions
|
||||
//========================================================================
|
||||
|
||||
/// Check if a vector is a unit vector
|
||||
|
||||
bool checkNorm(const Vector3& x, double tol = 1e-3);
|
||||
|
||||
/// Check if vector contains NaN values
|
||||
|
||||
bool hasNaN(const Vector3& vec);
|
||||
|
||||
/// Create a block diagonal matrix from two matrices
|
||||
|
||||
Matrix blockDiag(const Matrix& A, const Matrix& B);
|
||||
|
||||
/// Repeat a block matrix n times along the diagonal
|
||||
|
||||
Matrix repBlock(const Matrix& A, int n);
|
||||
|
||||
// Utility Functions Implementation
|
||||
|
||||
/**
|
||||
* @brief Verifies if a vector has unit norm within tolerance
|
||||
* @param x 3d vector
|
||||
* @param tol optional tolerance
|
||||
* @return Bool indicating that the vector norm is approximately 1
|
||||
*/
|
||||
bool checkNorm(const Vector3& x, double tol) {
|
||||
return abs(x.norm() - 1) < tol || std::isnan(x.norm());
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Checks if the input vector has any NaNs
|
||||
* @param vec A 3-D vector
|
||||
* @return true if present, false otherwise
|
||||
*/
|
||||
bool hasNaN(const Vector3& vec) {
|
||||
return std::isnan(vec[0]) || std::isnan(vec[1]) || std::isnan(vec[2]);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Creates a block diagonal matrix from input matrices
|
||||
* @param A Matrix A
|
||||
* @param B Matrix B
|
||||
* @return A single consolidated matrix with A in the top left and B in the
|
||||
* bottom right
|
||||
*/
|
||||
Matrix blockDiag(const Matrix& A, const Matrix& B) {
|
||||
if (A.size() == 0) {
|
||||
return B;
|
||||
} else if (B.size() == 0) {
|
||||
return A;
|
||||
} else {
|
||||
Matrix result(A.rows() + B.rows(), A.cols() + B.cols());
|
||||
result.setZero();
|
||||
result.block(0, 0, A.rows(), A.cols()) = A;
|
||||
result.block(A.rows(), A.cols(), B.rows(), B.cols()) = B;
|
||||
return result;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Creates a block diagonal matrix by repeating a matrix 'n' times
|
||||
* @param A A matrix
|
||||
* @param n Number of times to be repeated
|
||||
* @return Block diag matrix with A repeated 'n' times
|
||||
*/
|
||||
Matrix repBlock(const Matrix& A, int n) {
|
||||
if (n <= 0) return Matrix();
|
||||
|
||||
Matrix result = A;
|
||||
for (int i = 1; i < n; i++) {
|
||||
result = blockDiag(result, A);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
//========================================================================
|
||||
// Core Data Types
|
||||
//========================================================================
|
||||
|
||||
/// Input struct for the Biased Attitude System
|
||||
|
||||
struct Input {
|
||||
Vector3 w; /// Angular velocity (3-vector)
|
||||
Matrix Sigma; /// Noise covariance (6x6 matrix)
|
||||
static Input random(); /// Random input
|
||||
Matrix3 W() const { /// Return w as a skew symmetric matrix
|
||||
return Rot3::Hat(w);
|
||||
}
|
||||
};
|
||||
|
||||
/// Measurement struct
|
||||
struct Measurement {
|
||||
Unit3 y; /// Measurement direction in sensor frame
|
||||
Unit3 d; /// Known direction in global frame
|
||||
Matrix3 Sigma; /// Covariance matrix of the measurement
|
||||
int cal_idx = -1; /// Calibration index (-1 for calibrated sensor)
|
||||
};
|
||||
|
||||
/// State class representing the state of the Biased Attitude System
|
||||
template <size_t N>
|
||||
class State {
|
||||
public:
|
||||
Rot3 R; // Attitude rotation matrix R
|
||||
Vector3 b; // Gyroscope bias b
|
||||
std::array<Rot3, N> S; // Sensor calibrations S
|
||||
|
||||
/// Constructor
|
||||
State(const Rot3& R = Rot3::Identity(), const Vector3& b = Vector3::Zero(),
|
||||
const std::array<Rot3, N>& S = std::array<Rot3, N>{})
|
||||
: R(R), b(b), S(S) {}
|
||||
|
||||
/// Identity function
|
||||
static State identity() {
|
||||
std::array<Rot3, N> S_id{};
|
||||
S_id.fill(Rot3::Identity());
|
||||
return State(Rot3::Identity(), Vector3::Zero(), S_id);
|
||||
}
|
||||
/**
|
||||
* Compute Local coordinates in the state relative to another state.
|
||||
* @param other The other state
|
||||
* @return Local coordinates in the tangent space
|
||||
*/
|
||||
Vector localCoordinates(const State<N>& other) const {
|
||||
Vector eps(6 + 3 * N);
|
||||
|
||||
// First 3 elements - attitude
|
||||
eps.head<3>() = Rot3::Logmap(R.between(other.R));
|
||||
// Next 3 elements - bias
|
||||
// Next 3 elements - bias
|
||||
eps.segment<3>(3) = other.b - b;
|
||||
|
||||
// Remaining elements - calibrations
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
eps.segment<3>(6 + 3 * i) = Rot3::Logmap(S[i].between(other.S[i]));
|
||||
}
|
||||
|
||||
return eps;
|
||||
}
|
||||
|
||||
/**
|
||||
* Retract from tangent space back to the manifold
|
||||
* @param v Vector in the tangent space
|
||||
* @return New state
|
||||
*/
|
||||
State retract(const Vector& v) const {
|
||||
if (v.size() != static_cast<Eigen::Index>(6 + 3 * N)) {
|
||||
throw std::invalid_argument(
|
||||
"Vector size does not match state dimensions");
|
||||
}
|
||||
Rot3 newR = R * Rot3::Expmap(v.head<3>());
|
||||
Vector3 newB = b + v.segment<3>(3);
|
||||
std::array<Rot3, N> newS;
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
newS[i] = S[i] * Rot3::Expmap(v.segment<3>(6 + 3 * i));
|
||||
}
|
||||
return State(newR, newB, newS);
|
||||
}
|
||||
};
|
||||
|
||||
//========================================================================
|
||||
// Symmetry Group
|
||||
//========================================================================
|
||||
|
||||
/**
|
||||
* Symmetry group (SO(3) |x so(3)) x SO(3) x ... x SO(3)
|
||||
* Each element of the B list is associated with a calibration state
|
||||
*/
|
||||
template <size_t N>
|
||||
struct G {
|
||||
Rot3 A; /// First SO(3) element
|
||||
Matrix3 a; /// so(3) element (skew-symmetric matrix)
|
||||
std::array<Rot3, N> B; /// List of SO(3) elements for calibration
|
||||
|
||||
/// Initialize the symmetry group G
|
||||
G(const Rot3& A = Rot3::Identity(), const Matrix3& a = Matrix3::Zero(),
|
||||
const std::array<Rot3, N>& B = std::array<Rot3, N>{})
|
||||
: A(A), a(a), B(B) {}
|
||||
|
||||
/// Group multiplication
|
||||
G operator*(const G<N>& other) const {
|
||||
std::array<Rot3, N> newB;
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
newB[i] = B[i] * other.B[i];
|
||||
}
|
||||
return G(A * other.A, a + Rot3::Hat(A.matrix() * Rot3::Vee(other.a)), newB);
|
||||
}
|
||||
|
||||
/// Group inverse
|
||||
G inv() const {
|
||||
Matrix3 Ainv = A.inverse().matrix();
|
||||
std::array<Rot3, N> Binv;
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
Binv[i] = B[i].inverse();
|
||||
}
|
||||
return G(A.inverse(), -Rot3::Hat(Ainv * Rot3::Vee(a)), Binv);
|
||||
}
|
||||
|
||||
/// Identity element
|
||||
static G identity(int n) {
|
||||
std::array<Rot3, N> B;
|
||||
B.fill(Rot3::Identity());
|
||||
return G(Rot3::Identity(), Matrix3::Zero(), B);
|
||||
}
|
||||
|
||||
/// Exponential map of the tangent space elements to the group
|
||||
static G exp(const Vector& x) {
|
||||
if (x.size() != static_cast<Eigen::Index>(6 + 3 * N)) {
|
||||
throw std::invalid_argument("Vector size mismatch for group exponential");
|
||||
}
|
||||
Rot3 A = Rot3::Expmap(x.head<3>());
|
||||
Vector3 a_vee = Rot3::ExpmapDerivative(-x.head<3>()) * x.segment<3>(3);
|
||||
Matrix3 a = Rot3::Hat(a_vee);
|
||||
std::array<Rot3, N> B;
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
B[i] = Rot3::Expmap(x.segment<3>(6 + 3 * i));
|
||||
}
|
||||
return G(A, a, B);
|
||||
}
|
||||
};
|
||||
} // namespace abc_eqf_lib
|
||||
|
||||
template <size_t N>
|
||||
struct traits<abc_eqf_lib::State<N>>
|
||||
: internal::LieGroupTraits<abc_eqf_lib::State<N>> {};
|
||||
|
||||
template <size_t N>
|
||||
struct traits<abc_eqf_lib::G<N>> : internal::LieGroupTraits<abc_eqf_lib::G<N>> {
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
#endif // ABC_H
|
||||
|
|
@ -2,290 +2,60 @@
|
|||
* @file ABC_EQF.h
|
||||
* @brief Header file for the Attitude-Bias-Calibration Equivariant Filter
|
||||
*
|
||||
* This file contains declarations for 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.
|
||||
* Authors: Darshan Rajasekaran & Jennifer Oum
|
||||
* This file contains declarations for 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. Authors: Darshan
|
||||
* Rajasekaran & Jennifer Oum
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ABC_EQF_H
|
||||
#define ABC_EQF_H
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <fstream>
|
||||
#include <functional>
|
||||
#include <iostream>
|
||||
#include <numeric> // For std::accumulate
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <functional>
|
||||
#include <chrono>
|
||||
#include <numeric> // For std::accumulate
|
||||
|
||||
#include "ABC.h"
|
||||
|
||||
// All implementations are wrapped in this namespace to avoid conflicts
|
||||
namespace gtsam {
|
||||
namespace abc_eqf_lib {
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// Global configuration
|
||||
// Define coordinate type: "EXPONENTIAL" or "NORMAL"
|
||||
extern const std::string COORDINATE;
|
||||
|
||||
//========================================================================
|
||||
// Utility Functions
|
||||
//========================================================================
|
||||
|
||||
|
||||
/// Check if a vector is a unit vector
|
||||
|
||||
bool checkNorm(const Vector3& x, double tol = 1e-3);
|
||||
|
||||
/// Check if vector contains NaN values
|
||||
|
||||
bool hasNaN(const Vector3& vec);
|
||||
|
||||
/// Create a block diagonal matrix from two matrices
|
||||
|
||||
Matrix blockDiag(const Matrix& A, const Matrix& B);
|
||||
|
||||
/// Repeat a block matrix n times along the diagonal
|
||||
|
||||
Matrix repBlock(const Matrix& A, int n);
|
||||
|
||||
/// Calculate numerical differential
|
||||
|
||||
Matrix numericalDifferential(std::function<Vector(const Vector&)> f, const Vector& x);
|
||||
|
||||
//========================================================================
|
||||
// Core Data Types
|
||||
//========================================================================
|
||||
|
||||
|
||||
|
||||
/// Input struct for the Biased Attitude System
|
||||
|
||||
struct Input {
|
||||
Vector3 w; /// Angular velocity (3-vector)
|
||||
Matrix Sigma; /// Noise covariance (6x6 matrix)
|
||||
static Input random(); /// Random input
|
||||
Matrix3 W() const { /// Return w as a skew symmetric matrix
|
||||
return Rot3::Hat(w);
|
||||
}
|
||||
static Input create(const Vector3& w, const Matrix& Sigma) { /// Initialize w and Sigma
|
||||
if (Sigma.rows() != 6 || Sigma.cols() != 6) {
|
||||
throw std::invalid_argument("Input measurement noise covariance must be 6x6");
|
||||
}
|
||||
/// Check positive semi-definite
|
||||
Eigen::SelfAdjointEigenSolver<Matrix> eigensolver(Sigma);
|
||||
if (eigensolver.eigenvalues().minCoeff() < -1e-10) {
|
||||
throw std::invalid_argument("Covariance matrix must be semi-positive definite");
|
||||
}
|
||||
|
||||
return Input{w, Sigma}; // use brace initialization
|
||||
}
|
||||
};
|
||||
|
||||
/// Measurement struct
|
||||
|
||||
struct Measurement {
|
||||
Unit3 y; /// Measurement direction in sensor frame
|
||||
Unit3 d; /// Known direction in global frame
|
||||
Matrix3 Sigma; /// Covariance matrix of the measurement
|
||||
int cal_idx = -1; /// Calibration index (-1 for calibrated sensor)
|
||||
static Measurement create(const Unit3& y_vec, const Unit3& d_vec, /// Initialize measurement
|
||||
const Matrix3& Sigma_in, int i) {
|
||||
/// Check positive semi-definite
|
||||
Eigen::SelfAdjointEigenSolver<Matrix3> eigensolver(Sigma_in);
|
||||
if (eigensolver.eigenvalues().minCoeff() < -1e-10) {
|
||||
throw std::invalid_argument("Covariance matrix must be semi-positive definite");
|
||||
}
|
||||
return Measurement{y_vec, Unit3(d_vec), Sigma_in, i}; // Brace initialization
|
||||
}
|
||||
};
|
||||
|
||||
/// State class representing the state of the Biased Attitude System
|
||||
|
||||
class State {
|
||||
public:
|
||||
Rot3 R; // Attitude rotation matrix R
|
||||
Vector3 b; // Gyroscope bias b
|
||||
std::vector<Rot3> S; // Sensor calibrations S
|
||||
|
||||
/// Constructor
|
||||
State(const Rot3& R = Rot3::Identity(),
|
||||
const Vector3& b = Vector3::Zero(),
|
||||
const std::vector<Rot3>& S = std::vector<Rot3>())
|
||||
: R(R), b(b), S(S) {}
|
||||
|
||||
/// Identity function
|
||||
static State identity(int n) {
|
||||
std::vector<Rot3> calibrations(n, Rot3::Identity());
|
||||
return State(Rot3::Identity(), Vector3::Zero(), calibrations);
|
||||
}
|
||||
/**
|
||||
* Compute Local coordinates in the state relative to another state.
|
||||
* @param other The other state
|
||||
* @return Local coordinates in the tangent space
|
||||
*/
|
||||
Vector localCoordinates(const State& other) const {
|
||||
Vector eps(6 + 3 * S.size());
|
||||
|
||||
// First 3 elements - attitude
|
||||
eps.head<3>() = Rot3::Logmap(R.between(other.R));
|
||||
// Next 3 elements - bias
|
||||
// Next 3 elements - bias
|
||||
eps.segment<3>(3) = other.b - b;
|
||||
|
||||
// Remaining elements - calibrations
|
||||
for (size_t i = 0; i < S.size(); i++) {
|
||||
eps.segment<3>(6 + 3*i) = Rot3::Logmap(S[i].between(other.S[i]));
|
||||
}
|
||||
|
||||
return eps;
|
||||
}
|
||||
|
||||
/**
|
||||
* Retract from tangent space back to the manifold
|
||||
* @param v Vector in the tangent space
|
||||
* @return New state
|
||||
*/
|
||||
State retract(const Vector& v) const {
|
||||
if (v.size() < 6 || v.size() % 3 != 0 || v.size() != 6 + 3 * static_cast<Eigen::Index>(S.size())) {
|
||||
throw std::invalid_argument("Vector size does not match state dimensions");
|
||||
}
|
||||
|
||||
// Modify attitude
|
||||
Rot3 newR = R * Rot3::Expmap(v.head<3>());
|
||||
|
||||
// Modify bias
|
||||
Vector3 newB = b + v.segment<3>(3);
|
||||
|
||||
// Modify calibrations
|
||||
std::vector<Rot3> newS;
|
||||
for (size_t i = 0; i < S.size(); i++) {
|
||||
newS.push_back(S[i] * Rot3::Expmap(v.segment<3>(6 + 3*i)));
|
||||
}
|
||||
|
||||
return State(newR, newB, newS);
|
||||
}
|
||||
};
|
||||
|
||||
/// Data structure for ground-truth, input and output data
|
||||
|
||||
struct Data {
|
||||
State xi; /// Ground-truth state
|
||||
int n_cal; /// Number of calibration states
|
||||
Input u; /// Input measurements
|
||||
std::vector<Measurement> y; /// Output measurements
|
||||
int n_meas; /// Number of measurements
|
||||
double t; /// Time
|
||||
double dt; /// Time step
|
||||
|
||||
static Data create(const State& xi, int n_cal, const Input& u, /// Initialize Data
|
||||
const std::vector<Measurement>& y, int n_meas,
|
||||
double t, double dt) {
|
||||
return Data{xi, n_cal, u, y, n_meas, t, dt}; /// Bracket initialization
|
||||
}
|
||||
};
|
||||
|
||||
//========================================================================
|
||||
// Symmetry Group
|
||||
//========================================================================
|
||||
|
||||
/**
|
||||
* Symmetry group (SO(3) |x so(3)) x SO(3) x ... x SO(3)
|
||||
* Each element of the B list is associated with a calibration state
|
||||
*/
|
||||
class G {
|
||||
public:
|
||||
Rot3 A; /// First SO(3) element
|
||||
Matrix3 a; /// so(3) element (skew-symmetric matrix)
|
||||
std::vector<Rot3> B; /// List of SO(3) elements for calibration
|
||||
|
||||
/// Initialize the symmetry group G
|
||||
G(const Rot3& A = Rot3::Identity(),
|
||||
const Matrix3& a = Matrix3::Zero(),
|
||||
const std::vector<Rot3>& B = std::vector<Rot3>())
|
||||
: A(A), a(a), B(B) {}
|
||||
|
||||
/// Group multiplication
|
||||
G operator*(const G& other) const {
|
||||
if (B.size() != other.B.size()) {
|
||||
throw std::invalid_argument("Group elements must have the same number of calibration elements");
|
||||
}
|
||||
|
||||
std::vector<Rot3> new_B;
|
||||
for (size_t i = 0; i < B.size(); i++) {
|
||||
new_B.push_back(B[i] * other.B[i]);
|
||||
}
|
||||
|
||||
return G(A * other.A,
|
||||
a + Rot3::Hat(A.matrix() * Rot3::Vee(other.a)),
|
||||
new_B);
|
||||
}
|
||||
|
||||
/// Group inverse
|
||||
G inv() const {
|
||||
Matrix3 A_inv = A.inverse().matrix();
|
||||
std::vector<Rot3> B_inv;
|
||||
for (const auto& b : B) {
|
||||
B_inv.push_back(b.inverse());
|
||||
}
|
||||
|
||||
return G(A.inverse(),
|
||||
-Rot3::Hat(A_inv * Rot3::Vee(a)),
|
||||
B_inv);
|
||||
}
|
||||
|
||||
/// Identity element
|
||||
static G identity(int n) {
|
||||
std::vector<Rot3> B(n, Rot3::Identity());
|
||||
return G(Rot3::Identity(), Matrix3::Zero(), B);
|
||||
}
|
||||
|
||||
/// Exponential map of the tangent space elements to the group
|
||||
static G exp(const Vector& x) {
|
||||
if (x.size() < 6 || x.size() % 3 != 0) {
|
||||
throw std::invalid_argument("Wrong size, a vector with size multiple of 3 and at least 6 must be provided");
|
||||
}
|
||||
|
||||
int n = (x.size() - 6) / 3;
|
||||
Rot3 A = Rot3::Expmap(x.head<3>());
|
||||
|
||||
Vector3 a_vee = Rot3::ExpmapDerivative(-x.head<3>()) * x.segment<3>(3);
|
||||
Matrix3 a = Rot3::Hat(a_vee);
|
||||
|
||||
std::vector<Rot3> B;
|
||||
for (int i = 0; i < n; i++) {
|
||||
B.push_back(Rot3::Expmap(x.segment<3>(6 + 3 * i)));
|
||||
}
|
||||
|
||||
return G(A, a, B);
|
||||
}
|
||||
};
|
||||
|
||||
//========================================================================
|
||||
// Helper Functions for EqF
|
||||
//========================================================================
|
||||
|
||||
/// Calculate numerical differential
|
||||
|
||||
Matrix numericalDifferential(std::function<Vector(const Vector&)> f,
|
||||
const Vector& x);
|
||||
|
||||
/**
|
||||
* Compute the lift of the system (Theorem 3.8, Equation 7)
|
||||
* @param xi State
|
||||
* @param u Input
|
||||
* @return Lift vector
|
||||
*/
|
||||
Vector lift(const State& xi, const Input& u);
|
||||
template <size_t N>
|
||||
Vector lift(const State<N>& xi, const Input& u);
|
||||
|
||||
/**
|
||||
* Action of the symmetry group on the state space (Equation 4)
|
||||
|
|
@ -293,7 +63,8 @@ Vector lift(const State& xi, const Input& u);
|
|||
* @param xi State
|
||||
* @return New state after group action
|
||||
*/
|
||||
State stateAction(const G& X, const State& xi);
|
||||
template <size_t N>
|
||||
State<N> operator*(const G<N>& X, const State<N>& xi);
|
||||
|
||||
/**
|
||||
* Action of the symmetry group on the input space (Equation 5)
|
||||
|
|
@ -301,7 +72,8 @@ State stateAction(const G& X, const State& xi);
|
|||
* @param u Input
|
||||
* @return New input after group action
|
||||
*/
|
||||
Input velocityAction(const G& X, const Input& u);
|
||||
template <size_t N>
|
||||
Input velocityAction(const G<N>& X, const Input& u);
|
||||
|
||||
/**
|
||||
* Action of the symmetry group on the output space (Equation 6)
|
||||
|
|
@ -310,28 +82,29 @@ Input velocityAction(const G& X, const Input& u);
|
|||
* @param idx Calibration index
|
||||
* @return New direction after group action
|
||||
*/
|
||||
Vector3 outputAction(const G& X, const Unit3& y, int idx);
|
||||
template <size_t N>
|
||||
Vector3 outputAction(const G<N>& X, const Unit3& y, int idx);
|
||||
|
||||
/**
|
||||
* Differential of the phi action at E = Id in local coordinates
|
||||
* @param xi State
|
||||
* @return Differential matrix
|
||||
*/
|
||||
Matrix stateActionDiff(const State& xi);
|
||||
template <size_t N>
|
||||
Matrix stateActionDiff(const State<N>& xi);
|
||||
|
||||
//========================================================================
|
||||
// Equivariant Filter (EqF)
|
||||
//========================================================================
|
||||
|
||||
/// Equivariant Filter (EqF) implementation
|
||||
|
||||
template <size_t N>
|
||||
class EqF {
|
||||
private:
|
||||
private:
|
||||
int dof; // Degrees of freedom
|
||||
int n_cal; // Number of calibration states
|
||||
G X_hat; // Filter state
|
||||
G<N> X_hat; // Filter state
|
||||
Matrix Sigma; // Error covariance
|
||||
State xi_0; // Origin state
|
||||
State<N> xi_0; // Origin state
|
||||
Matrix Dphi0; // Differential of phi at origin
|
||||
Matrix InnovationLift; // Innovation lift matrix
|
||||
|
||||
|
|
@ -371,20 +144,19 @@ private:
|
|||
*/
|
||||
Matrix outputMatrixDt(int idx) const;
|
||||
|
||||
public:
|
||||
public:
|
||||
/**
|
||||
* Initialize EqF
|
||||
* @param Sigma Initial covariance
|
||||
* @param n Number of calibration states
|
||||
* @param m Number of sensors
|
||||
*/
|
||||
EqF(const Matrix& Sigma, int n, int m);
|
||||
EqF(const Matrix& Sigma, int m);
|
||||
|
||||
/**
|
||||
* Return estimated state
|
||||
* @return Current state estimate
|
||||
*/
|
||||
State stateEstimate() const;
|
||||
State<N> stateEstimate() const;
|
||||
|
||||
/**
|
||||
* Propagate the filter state
|
||||
|
|
@ -400,98 +172,6 @@ public:
|
|||
void update(const Measurement& y);
|
||||
};
|
||||
|
||||
// Global configuration
|
||||
const std::string COORDINATE = "EXPONENTIAL"; // Denotes how the states are mapped to the vector representations
|
||||
|
||||
//========================================================================
|
||||
// Utility Functions
|
||||
//========================================================================
|
||||
/**
|
||||
* @brief Verifies if a vector has unit norm within tolerance
|
||||
* @param x 3d vector
|
||||
* @param tol optional tolerance
|
||||
* @return Bool indicating that the vector norm is approximately 1
|
||||
* Uses Vector3 norm() method to calculate vector magnitude
|
||||
*/
|
||||
bool checkNorm(const Vector3& x, double tol) {
|
||||
return abs(x.norm() - 1) < tol || std::isnan(x.norm());
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Checks if the input vector has any NaNs
|
||||
* @param vec A 3-D vector
|
||||
* @return true if present, false otherwise
|
||||
*/
|
||||
bool hasNaN(const Vector3& vec) {
|
||||
return std::isnan(vec[0]) || std::isnan(vec[1]) || std::isnan(vec[2]);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Creates a block diagonal matrix from input matrices
|
||||
* @param A Matrix A
|
||||
* @param B Matrix B
|
||||
* @return A single consolidated matrix with A in the top left and B in the
|
||||
* bottom right
|
||||
* Uses Matrix's rows(), cols(), setZero(), and block() methods
|
||||
*/
|
||||
|
||||
Matrix blockDiag(const Matrix& A, const Matrix& B) {
|
||||
if (A.size() == 0) {
|
||||
return B;
|
||||
} else if (B.size() == 0) {
|
||||
return A;
|
||||
} else {
|
||||
Matrix result(A.rows() + B.rows(), A.cols() + B.cols());
|
||||
result.setZero();
|
||||
result.block(0, 0, A.rows(), A.cols()) = A;
|
||||
result.block(A.rows(), A.cols(), B.rows(), B.cols()) = B;
|
||||
return result;
|
||||
}
|
||||
}
|
||||
/**
|
||||
* @brief Creates a block diagonal matrix by repeating a matrix 'n' times
|
||||
* @param A A matrix
|
||||
* @param n Number of times to be repeated
|
||||
* @return Block diag matrix with A repeated 'n' times
|
||||
* Recursively uses blockDiag() function
|
||||
*/
|
||||
Matrix repBlock(const Matrix& A, int n) {
|
||||
if (n <= 0) return Matrix();
|
||||
|
||||
Matrix result = A;
|
||||
for (int i = 1; i < n; i++) {
|
||||
result = blockDiag(result, A);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Calculates the Jacobian matrix using central difference approximation
|
||||
* @param f Vector function f
|
||||
* @param x The point at which Jacobian is evaluated
|
||||
* @return Matrix containing numerical partial derivatives of f at x
|
||||
* Uses Vector's size() and Zero(), Matrix's Zero() and col() methods
|
||||
*/
|
||||
Matrix numericalDifferential(std::function<Vector(const Vector&)> f, const Vector& x) {
|
||||
double h = 1e-6;
|
||||
Vector fx = f(x);
|
||||
int n = fx.size();
|
||||
int m = x.size();
|
||||
Matrix Df = Matrix::Zero(n, m);
|
||||
|
||||
for (int j = 0; j < m; j++) {
|
||||
Vector ej = Vector::Zero(m);
|
||||
ej(j) = 1.0;
|
||||
|
||||
Vector fplus = f(x + h * ej);
|
||||
Vector fminus = f(x - h * ej);
|
||||
|
||||
Df.col(j) = (fplus - fminus) / (2*h);
|
||||
}
|
||||
|
||||
return Df;
|
||||
}
|
||||
|
||||
//========================================================================
|
||||
// Helper Functions Implementation
|
||||
//========================================================================
|
||||
|
|
@ -503,9 +183,9 @@ Matrix numericalDifferential(std::function<Vector(const Vector&)> f, const Vecto
|
|||
* @return Lifted input in Lie Algebra
|
||||
* Uses Vector zero & Rot3 inverse, matrix functions
|
||||
*/
|
||||
Vector lift(const State& xi, const Input& u) {
|
||||
int n = xi.S.size();
|
||||
Vector L = Vector::Zero(6 + 3 * n);
|
||||
template <size_t N>
|
||||
Vector lift(const State<N>& xi, const Input& u) {
|
||||
Vector L = Vector::Zero(6 + 3 * N);
|
||||
|
||||
// First 3 elements
|
||||
L.head<3>() = u.w - xi.b;
|
||||
|
|
@ -514,8 +194,8 @@ Vector lift(const State& xi, const Input& u) {
|
|||
L.segment<3>(3) = -u.W() * xi.b;
|
||||
|
||||
// Remaining elements
|
||||
for (int i = 0; i < n; i++) {
|
||||
L.segment<3>(6 + 3*i) = xi.S[i].inverse().matrix() * L.head<3>();
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
L.segment<3>(6 + 3 * i) = xi.S[i].inverse().matrix() * L.head<3>();
|
||||
}
|
||||
|
||||
return L;
|
||||
|
|
@ -533,18 +213,15 @@ Vector lift(const State& xi, const Input& u) {
|
|||
* @return Transformed state
|
||||
* Uses the Rot3 inverse and Vee functions
|
||||
*/
|
||||
State stateAction(const G& X, const State& xi) {
|
||||
if (xi.S.size() != X.B.size()) {
|
||||
throw std::invalid_argument("Number of calibration states and B elements must match");
|
||||
template <size_t N>
|
||||
State<N> operator*(const G<N>& X, const State<N>& xi) {
|
||||
std::array<Rot3, N> new_S;
|
||||
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
new_S[i] = X.A.inverse() * xi.S[i] * X.B[i];
|
||||
}
|
||||
|
||||
std::vector<Rot3> new_S;
|
||||
for (size_t i = 0; i < X.B.size(); i++) {
|
||||
new_S.push_back(X.A.inverse() * xi.S[i] * X.B[i]);
|
||||
}
|
||||
|
||||
return State(xi.R * X.A,
|
||||
X.A.inverse().matrix() * (xi.b - Rot3::Vee(X.a)),
|
||||
return State<N>(xi.R * X.A, X.A.inverse().matrix() * (xi.b - Rot3::Vee(X.a)),
|
||||
new_S);
|
||||
}
|
||||
/**
|
||||
|
|
@ -555,7 +232,8 @@ State stateAction(const G& X, const State& xi) {
|
|||
* Uses Rot3 Inverse, matrix and Vee functions and is critical for maintaining
|
||||
* the input equivariance
|
||||
*/
|
||||
Input velocityAction(const G& X, const Input& u) {
|
||||
template <size_t N>
|
||||
Input velocityAction(const G<N>& X, const Input& u) {
|
||||
return Input{X.A.inverse().matrix() * (u.w - Rot3::Vee(X.a)), u.Sigma};
|
||||
}
|
||||
/**
|
||||
|
|
@ -566,11 +244,12 @@ Input velocityAction(const G& X, const Input& u) {
|
|||
* @return Transformed direction
|
||||
* Uses Rot3 inverse, matric and Unit3 unitvector functions
|
||||
*/
|
||||
Vector3 outputAction(const G& X, const Unit3& y, int idx) {
|
||||
template <size_t N>
|
||||
Vector3 outputAction(const G<N>& X, const Unit3& y, int idx) {
|
||||
if (idx == -1) {
|
||||
return X.A.inverse().matrix() * y.unitVector();
|
||||
} else {
|
||||
if (idx >= static_cast<int>(X.B.size())) {
|
||||
if (idx >= static_cast<int>(N)) {
|
||||
throw std::out_of_range("Calibration index out of range");
|
||||
}
|
||||
return X.B[idx].inverse().matrix() * y.unitVector();
|
||||
|
|
@ -578,52 +257,33 @@ Vector3 outputAction(const G& X, const Unit3& y, int idx) {
|
|||
}
|
||||
|
||||
/**
|
||||
* Maps the error states to vector representations through exponential
|
||||
* coordinates
|
||||
* @param e error state
|
||||
* @return Vector with local coordinates
|
||||
* Uses Rot3 logamo for mapping rotations to the tangent space
|
||||
* @brief Calculates the Jacobian matrix using central difference approximation
|
||||
* @param f Vector function f
|
||||
* @param x The point at which Jacobian is evaluated
|
||||
* @return Matrix containing numerical partial derivatives of f at x
|
||||
* Uses Vector's size() and Zero(), Matrix's Zero() and col() methods
|
||||
*/
|
||||
// Vector local_coords(const State& e) {
|
||||
// if (COORDINATE == "EXPONENTIAL") {
|
||||
// Vector eps(6 + 3 * e.S.size());
|
||||
//
|
||||
// // First 3 elements
|
||||
// eps.head<3>() = Rot3::Logmap(e.R);
|
||||
//
|
||||
// // Next 3 elements
|
||||
// eps.segment<3>(3) = e.b;
|
||||
//
|
||||
// // Remaining elements
|
||||
// for (size_t i = 0; i < e.S.size(); i++) {
|
||||
// eps.segment<3>(6 + 3*i) = Rot3::Logmap(e.S[i]);
|
||||
// }
|
||||
//
|
||||
// return eps;
|
||||
// } else if (COORDINATE == "NORMAL") {
|
||||
// throw std::runtime_error("Normal coordinate representation is not implemented yet");
|
||||
// } else {
|
||||
// throw std::invalid_argument("Invalid coordinate representation");
|
||||
// }
|
||||
// }
|
||||
/**
|
||||
* Used to convert the vectorized errors back to state space
|
||||
* @param eps Local coordinates in the exponential parameterization
|
||||
* @return State object corresponding to these coordinates
|
||||
* Uses Rot3 expmap through the G::exp() function
|
||||
*/
|
||||
// State local_coords_inv(const Vector& eps) {
|
||||
// G X = G::exp(eps);
|
||||
//
|
||||
// if (COORDINATE == "EXPONENTIAL") {
|
||||
// std::vector<Rot3> S = X.B;
|
||||
// return State(X.A, eps.segment<3>(3), S);
|
||||
// } else if (COORDINATE == "NORMAL") {
|
||||
// throw std::runtime_error("Normal coordinate representation is not implemented yet");
|
||||
// } else {
|
||||
// throw std::invalid_argument("Invalid coordinate representation");
|
||||
// }
|
||||
// }
|
||||
Matrix numericalDifferential(std::function<Vector(const Vector&)> f,
|
||||
const Vector& x) {
|
||||
double h = 1e-6;
|
||||
Vector fx = f(x);
|
||||
int n = fx.size();
|
||||
int m = x.size();
|
||||
Matrix Df = Matrix::Zero(n, m);
|
||||
|
||||
for (int j = 0; j < m; j++) {
|
||||
Vector ej = Vector::Zero(m);
|
||||
ej(j) = 1.0;
|
||||
|
||||
Vector fplus = f(x + h * ej);
|
||||
Vector fminus = f(x - h * ej);
|
||||
|
||||
Df.col(j) = (fplus - fminus) / (2 * h);
|
||||
}
|
||||
|
||||
return Df;
|
||||
}
|
||||
|
||||
/**
|
||||
* Computes the differential of a state action at the identity of the symmetry
|
||||
* group
|
||||
|
|
@ -632,15 +292,15 @@ Vector3 outputAction(const G& X, const Unit3& y, int idx) {
|
|||
* @return A matrix representing the jacobian of the state action
|
||||
* Uses numericalDifferential, and Rot3 expmap, logmap
|
||||
*/
|
||||
Matrix stateActionDiff(const State& xi) {
|
||||
std::function<Vector(const Vector&)> coordsAction =
|
||||
[&xi](const Vector& U) {
|
||||
G groupElement = G::exp(U);
|
||||
State transformed = stateAction(groupElement, xi);
|
||||
template <size_t N>
|
||||
Matrix stateActionDiff(const State<N>& xi) {
|
||||
std::function<Vector(const Vector&)> coordsAction = [&xi](const Vector& U) {
|
||||
G<N> groupElement = G<N>::exp(U);
|
||||
State<N> transformed = groupElement * xi;
|
||||
return xi.localCoordinates(transformed);
|
||||
};
|
||||
|
||||
Vector zeros = Vector::Zero(6 + 3 * xi.S.size());
|
||||
Vector zeros = Vector::Zero(6 + 3 * N);
|
||||
Matrix differential = numericalDifferential(coordsAction, zeros);
|
||||
return differential;
|
||||
}
|
||||
|
|
@ -656,26 +316,32 @@ Matrix stateActionDiff(const State& xi) {
|
|||
* @param m Number of sensors
|
||||
* Uses SelfAdjointSolver, completeOrthoganalDecomposition().pseudoInverse()
|
||||
*/
|
||||
EqF::EqF(const Matrix& Sigma, int n, int m)
|
||||
: dof(6 + 3 * n), n_cal(n), X_hat(G::identity(n)),
|
||||
Sigma(Sigma), xi_0(State::identity(n)) {
|
||||
|
||||
template <size_t N>
|
||||
EqF<N>::EqF(const Matrix& Sigma, int m)
|
||||
: dof(6 + 3 * N),
|
||||
X_hat(G<N>::identity(N)),
|
||||
Sigma(Sigma),
|
||||
xi_0(State<N>::identity()) {
|
||||
if (Sigma.rows() != dof || Sigma.cols() != dof) {
|
||||
throw std::invalid_argument("Initial covariance dimensions must match the degrees of freedom");
|
||||
throw std::invalid_argument(
|
||||
"Initial covariance dimensions must match the degrees of freedom");
|
||||
}
|
||||
|
||||
// Check positive semi-definite
|
||||
Eigen::SelfAdjointEigenSolver<Matrix> eigensolver(Sigma);
|
||||
if (eigensolver.eigenvalues().minCoeff() < -1e-10) {
|
||||
throw std::invalid_argument("Covariance matrix must be semi-positive definite");
|
||||
throw std::invalid_argument(
|
||||
"Covariance matrix must be semi-positive definite");
|
||||
}
|
||||
|
||||
if (n < 0) {
|
||||
throw std::invalid_argument("Number of calibration states must be non-negative");
|
||||
if (N < 0) {
|
||||
throw std::invalid_argument(
|
||||
"Number of calibration states must be non-negative");
|
||||
}
|
||||
|
||||
if (m <= 1) {
|
||||
throw std::invalid_argument("Number of direction sensors must be at least 2");
|
||||
throw std::invalid_argument(
|
||||
"Number of direction sensors must be at least 2");
|
||||
}
|
||||
|
||||
// Compute differential of phi
|
||||
|
|
@ -686,8 +352,9 @@ EqF::EqF(const Matrix& Sigma, int n, int m)
|
|||
* Computes the internal group state to a physical state estimate
|
||||
* @return Current state estimate
|
||||
*/
|
||||
State EqF::stateEstimate() const {
|
||||
return stateAction(X_hat, xi_0);
|
||||
template <size_t N>
|
||||
State<N> EqF<N>::stateEstimate() const {
|
||||
return X_hat * xi_0;
|
||||
}
|
||||
/**
|
||||
* Implements the prediction step of the EqF using system dynamics and
|
||||
|
|
@ -697,18 +364,18 @@ State EqF::stateEstimate() const {
|
|||
* @param dt time steps
|
||||
* Updated internal state and covariance
|
||||
*/
|
||||
void EqF::propagation(const Input& u, double dt) {
|
||||
State state_est = stateEstimate();
|
||||
template <size_t N>
|
||||
void EqF<N>::propagation(const Input& u, double dt) {
|
||||
State<N> state_est = stateEstimate();
|
||||
Vector L = lift(state_est, u);
|
||||
|
||||
Matrix Phi_DT = stateTransitionMatrix(u, dt);
|
||||
Matrix Bt = inputMatrixBt();
|
||||
|
||||
Matrix tempSigma = blockDiag(u.Sigma,
|
||||
repBlock(1e-9 * Matrix3::Identity(), n_cal));
|
||||
Matrix tempSigma = blockDiag(u.Sigma, repBlock(1e-9 * I_3x3, N));
|
||||
Matrix M_DT = (Bt * tempSigma * Bt.transpose()) * dt;
|
||||
|
||||
X_hat = X_hat * G::exp(L * dt);
|
||||
X_hat = X_hat * G<N>::exp(L * dt);
|
||||
Sigma = Phi_DT * Sigma * Phi_DT.transpose() + M_DT;
|
||||
}
|
||||
/**
|
||||
|
|
@ -718,8 +385,9 @@ void EqF::propagation(const Input& u, double dt) {
|
|||
*
|
||||
* @param y Measurements
|
||||
*/
|
||||
void EqF::update(const Measurement& y) {
|
||||
if (y.cal_idx > n_cal) {
|
||||
template <size_t N>
|
||||
void EqF<N>::update(const Measurement& y) {
|
||||
if (y.cal_idx > static_cast<int>(N)) {
|
||||
throw std::invalid_argument("Calibration index out of range");
|
||||
}
|
||||
|
||||
|
|
@ -740,7 +408,7 @@ void EqF::update(const Measurement& y) {
|
|||
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;
|
||||
X_hat = G<N>::exp(Delta) * X_hat;
|
||||
Sigma = (Matrix::Identity(dof, dof) - K * Ct) * Sigma;
|
||||
}
|
||||
/**
|
||||
|
|
@ -749,20 +417,14 @@ void EqF::update(const Measurement& y) {
|
|||
* @return Linearized state matrix
|
||||
* Uses Matrix zero and Identity functions
|
||||
*/
|
||||
Matrix EqF::stateMatrixA(const Input& u) const {
|
||||
template <size_t N>
|
||||
Matrix EqF<N>::stateMatrixA(const Input& u) const {
|
||||
Matrix3 W0 = velocityAction(X_hat.inv(), u).W();
|
||||
Matrix A1 = Matrix::Zero(6, 6);
|
||||
|
||||
if (COORDINATE == "EXPONENTIAL") {
|
||||
A1.block<3, 3>(0, 3) = -Matrix3::Identity();
|
||||
A1.block<3, 3>(0, 3) = -I_3x3;
|
||||
A1.block<3, 3>(3, 3) = W0;
|
||||
Matrix A2 = repBlock(W0, n_cal);
|
||||
Matrix A2 = repBlock(W0, N);
|
||||
return blockDiag(A1, A2);
|
||||
} else if (COORDINATE == "NORMAL") {
|
||||
throw std::runtime_error("Normal coordinate representation is not implemented yet");
|
||||
} else {
|
||||
throw std::invalid_argument("Invalid coordinate representation");
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -771,49 +433,35 @@ Matrix EqF::stateMatrixA(const Input& u) const {
|
|||
* @param dt time step
|
||||
* @return State transition matrix in discrete time
|
||||
*/
|
||||
Matrix EqF::stateTransitionMatrix(const Input& u, double dt) const {
|
||||
template <size_t N>
|
||||
Matrix EqF<N>::stateTransitionMatrix(const Input& u, double dt) const {
|
||||
Matrix3 W0 = velocityAction(X_hat.inv(), u).W();
|
||||
Matrix Phi1 = Matrix::Zero(6, 6);
|
||||
|
||||
Matrix3 Phi12 = -dt * (Matrix3::Identity() + (dt / 2) * W0 + ((dt*dt) / 6) * W0 * W0);
|
||||
Matrix3 Phi22 = Matrix3::Identity() + dt * W0 + ((dt*dt) / 2) * W0 * W0;
|
||||
Matrix3 Phi12 = -dt * (I_3x3 + (dt / 2) * W0 + ((dt * dt) / 6) * W0 * W0);
|
||||
Matrix3 Phi22 = I_3x3 + dt * W0 + ((dt * dt) / 2) * W0 * W0;
|
||||
|
||||
if (COORDINATE == "EXPONENTIAL") {
|
||||
Phi1.block<3, 3>(0, 0) = Matrix3::Identity();
|
||||
Phi1.block<3, 3>(0, 0) = I_3x3;
|
||||
Phi1.block<3, 3>(0, 3) = Phi12;
|
||||
Phi1.block<3, 3>(3, 3) = Phi22;
|
||||
Matrix Phi2 = repBlock(Phi22, n_cal);
|
||||
Matrix Phi2 = repBlock(Phi22, N);
|
||||
return blockDiag(Phi1, Phi2);
|
||||
} else if (COORDINATE == "NORMAL") {
|
||||
throw std::runtime_error("Normal coordinate representation is not implemented yet");
|
||||
} else {
|
||||
throw std::invalid_argument("Invalid coordinate representation");
|
||||
}
|
||||
}
|
||||
/**
|
||||
* Computes the input uncertainty propagation matrix
|
||||
* @return
|
||||
* Uses the blockdiag matrix
|
||||
*/
|
||||
Matrix EqF::inputMatrixBt() const {
|
||||
if (COORDINATE == "EXPONENTIAL") {
|
||||
template <size_t N>
|
||||
Matrix EqF<N>::inputMatrixBt() const {
|
||||
Matrix B1 = blockDiag(X_hat.A.matrix(), X_hat.A.matrix());
|
||||
Matrix B2;
|
||||
Matrix B2(3 * N, 3 * N);
|
||||
|
||||
for (const auto& B : X_hat.B) {
|
||||
if (B2.size() == 0) {
|
||||
B2 = B.matrix();
|
||||
} else {
|
||||
B2 = blockDiag(B2, B.matrix());
|
||||
}
|
||||
for (size_t i = 0; i < N; ++i) {
|
||||
B2.block<3, 3>(3 * i, 3 * i) = X_hat.B[i].matrix();
|
||||
}
|
||||
|
||||
return blockDiag(B1, B2);
|
||||
} else if (COORDINATE == "NORMAL") {
|
||||
throw std::runtime_error("Normal coordinate representation is not implemented yet");
|
||||
} else {
|
||||
throw std::invalid_argument("Invalid coordinate representation");
|
||||
}
|
||||
}
|
||||
/**
|
||||
* Computes the linearized measurement matrix. The structure depends on whether
|
||||
|
|
@ -823,8 +471,9 @@ Matrix EqF::inputMatrixBt() const {
|
|||
* @return Measurement matrix
|
||||
* Uses the matrix zero, Rot3 hat and the Unitvector functions
|
||||
*/
|
||||
Matrix EqF::measurementMatrixC(const Unit3& d, int idx) const {
|
||||
Matrix Cc = Matrix::Zero(3, 3 * n_cal);
|
||||
template <size_t N>
|
||||
Matrix EqF<N>::measurementMatrixC(const Unit3& d, int idx) const {
|
||||
Matrix Cc = Matrix::Zero(3, 3 * N);
|
||||
|
||||
// If the measurement is related to a sensor that has a calibration state
|
||||
if (idx >= 0) {
|
||||
|
|
@ -835,10 +484,10 @@ Matrix EqF::measurementMatrixC(const Unit3& d, int idx) const {
|
|||
Matrix3 wedge_d = Rot3::Hat(d.unitVector());
|
||||
|
||||
// Create the combined matrix
|
||||
Matrix temp(3, 6 + 3 * n_cal);
|
||||
Matrix temp(3, 6 + 3 * N);
|
||||
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;
|
||||
temp.block(0, 6, 3, 3 * N) = Cc;
|
||||
|
||||
return wedge_d * temp;
|
||||
}
|
||||
|
|
@ -847,10 +496,11 @@ Matrix EqF::measurementMatrixC(const Unit3& d, int idx) const {
|
|||
* @param idx Calibration index
|
||||
* @return Returns B[idx] for calibrated sensors, A for uncalibrated
|
||||
*/
|
||||
Matrix EqF::outputMatrixDt(int idx) const {
|
||||
template <size_t N>
|
||||
Matrix EqF<N>::outputMatrixDt(int idx) const {
|
||||
// If the measurement is related to a sensor that has a calibration state
|
||||
if (idx >= 0) {
|
||||
if (idx >= static_cast<int>(X_hat.B.size())) {
|
||||
if (idx >= static_cast<int>(N)) {
|
||||
throw std::out_of_range("Calibration index out of range");
|
||||
}
|
||||
return X_hat.B[idx].matrix();
|
||||
|
|
@ -859,9 +509,11 @@ Matrix EqF::outputMatrixDt(int idx) const {
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
} // namespace abc_eqf_lib
|
||||
|
||||
template <size_t N>
|
||||
struct traits<abc_eqf_lib::EqF<N>>
|
||||
: internal::LieGroupTraits<abc_eqf_lib::EqF<N>> {};
|
||||
} // namespace gtsam
|
||||
|
||||
#endif // ABC_EQF_H
|
||||
|
|
@ -3,17 +3,33 @@
|
|||
* @brief Demonstration of the full 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.
|
||||
* Authors: Darshan Rajasekaran & Jennifer Oum
|
||||
* 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. Authors: Darshan
|
||||
* Rajasekaran & Jennifer Oum
|
||||
*/
|
||||
|
||||
#include "ABC_EQF.h"
|
||||
|
||||
// Use namespace for convenience
|
||||
using namespace abc_eqf_lib;
|
||||
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;
|
||||
|
||||
/// Data structure for ground-truth, input and output data
|
||||
struct Data {
|
||||
M xi; /// Ground-truth state
|
||||
Input u; /// Input measurements
|
||||
std::vector<Measurement> y; /// Output measurements
|
||||
int n_meas; /// Number of measurements
|
||||
double t; /// Time
|
||||
double dt; /// Time step
|
||||
};
|
||||
|
||||
//========================================================================
|
||||
// Data Processing Functions
|
||||
|
|
@ -31,47 +47,32 @@ using namespace gtsam;
|
|||
* - std_w_x, std_w_y, std_w_z: Angular velocity measurement standard deviations
|
||||
* - std_b_x, std_b_y, std_b_z: Bias process noise standard deviations
|
||||
* - y_x_0, y_y_0, y_z_0, y_x_1, y_y_1, y_z_1: Direction measurements
|
||||
* - std_y_x_0, std_y_y_0, std_y_z_0, std_y_x_1, std_y_y_1, std_y_z_1: Direction measurement standard deviations
|
||||
* - std_y_x_0, std_y_y_0, std_y_z_0, std_y_x_1, std_y_y_1, std_y_z_1: Direction
|
||||
* measurement standard deviations
|
||||
* - d_x_0, d_y_0, d_z_0, d_x_1, d_y_1, d_z_1: Reference directions
|
||||
*
|
||||
* @param filename Path to the CSV file
|
||||
* @param startRow First row to load (default: 0)
|
||||
* @param maxRows Maximum number of rows to load (default: all)
|
||||
* @param downsample Downsample factor (default: 1, which means no downsampling)
|
||||
* @return Vector of Data objects
|
||||
*/
|
||||
std::vector<Data> loadDataFromCSV(const std::string& filename,
|
||||
int startRow = 0,
|
||||
int maxRows = -1,
|
||||
int downsample = 1);
|
||||
std::vector<Data> loadDataFromCSV(const std::string& filename, int startRow = 0,
|
||||
int maxRows = -1, int downsample = 1);
|
||||
|
||||
/**
|
||||
* Process data with EqF and print summary results
|
||||
* @param filter Initialized EqF filter
|
||||
* @param data_list Vector of Data objects to process
|
||||
* @param printInterval Progress indicator interval (used internally)
|
||||
*/
|
||||
void processDataWithEqF(EqF& filter, const std::vector<Data>& data_list, int printInterval = 10);
|
||||
/// Process data with EqF and print summary results
|
||||
void processDataWithEqF(EqFilter& filter, const std::vector<Data>& data_list,
|
||||
int printInterval = 10);
|
||||
|
||||
//========================================================================
|
||||
// Data Processing Functions Implementation
|
||||
//========================================================================
|
||||
|
||||
/**
|
||||
* @brief Loads the test data from the csv file
|
||||
* @param filename path to the csv file is specified
|
||||
* @param startRow First row to load based on csv, 0 by default
|
||||
* @param maxRows maximum rows to load, defaults to all rows
|
||||
* @param downsample Downsample factor, default 1
|
||||
* @return A list of data objects
|
||||
*/
|
||||
/*
|
||||
* 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> loadDataFromCSV(const std::string& filename, int startRow,
|
||||
int maxRows, int downsample) {
|
||||
std::vector<Data> data_list;
|
||||
std::ifstream file(filename);
|
||||
|
||||
|
|
@ -136,16 +137,18 @@ std::vector<Data> loadDataFromCSV(const std::string& filename,
|
|||
|
||||
Vector3 b(values[5], values[6], values[7]);
|
||||
|
||||
Quaternion calQuat(values[8], values[9], values[10], values[11]); // w, x, y, z
|
||||
std::vector<Rot3> S = {Rot3(calQuat)};
|
||||
Quaternion calQuat(values[8], values[9], values[10],
|
||||
values[11]); // w, x, y, z
|
||||
std::array<Rot3, N> S = {Rot3(calQuat)};
|
||||
|
||||
State xi(R, b, S);
|
||||
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
|
||||
// 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
|
||||
|
|
@ -194,7 +197,7 @@ std::vector<Data> loadDataFromCSV(const std::string& filename,
|
|||
measurements.push_back(Measurement{Unit3(y1), Unit3(d1), covY1, -1});
|
||||
|
||||
// Create Data object and add to list
|
||||
data_list.push_back(Data{xi, 1, u, measurements, 2, t, dt});
|
||||
data_list.push_back(Data{xi, u, measurements, 2, t, dt});
|
||||
|
||||
rowCount++;
|
||||
|
||||
|
|
@ -215,21 +218,17 @@ std::vector<Data> loadDataFromCSV(const std::string& filename,
|
|||
|
||||
return data_list;
|
||||
}
|
||||
/**
|
||||
* @brief Takes in the data and runs an EqF on it and reports the results
|
||||
* @param filter Initialized EqF filter
|
||||
* @param data_list std::vector<Data>
|
||||
* @param printInterval Progress indicator
|
||||
* Prints the performance statstics like average error etc
|
||||
* Uses Rot3 between, logmap and rpy functions
|
||||
*/
|
||||
void processDataWithEqF(EqF& filter, const std::vector<Data>& data_list, int printInterval) {
|
||||
|
||||
/// Takes in the data and runs an EqF on it and reports the results
|
||||
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;
|
||||
std::cout << "Processing " << data_list.size() << " data points with EqF..."
|
||||
<< std::endl;
|
||||
|
||||
// Track performance metrics
|
||||
std::vector<double> att_errors;
|
||||
|
|
@ -264,8 +263,9 @@ void processDataWithEqF(EqF& filter, const std::vector<Data>& data_list, int pri
|
|||
// 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])) {
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
@ -273,13 +273,13 @@ void processDataWithEqF(EqF& filter, const std::vector<Data>& data_list, int pri
|
|||
filter.update(y);
|
||||
validMeasurements++;
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "Error updating at t=" << data.t
|
||||
<< ": " << e.what() << std::endl;
|
||||
std::cerr << "Error updating at t=" << data.t << ": " << e.what()
|
||||
<< std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
// Get current state estimate
|
||||
State estimate = filter.stateEstimate();
|
||||
M estimate = filter.stateEstimate();
|
||||
|
||||
// Calculate errors
|
||||
Vector3 att_error = Rot3::Logmap(data.xi.R.between(estimate.R));
|
||||
|
|
@ -311,64 +311,72 @@ void processDataWithEqF(EqF& filter, const std::vector<Data>& data_list, int pri
|
|||
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();
|
||||
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();
|
||||
State final_estimate = filter.stateEstimate();
|
||||
Vector3 final_att_error = Rot3::Logmap(final_data.xi.R.between(final_estimate.R));
|
||||
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]));
|
||||
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;
|
||||
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;
|
||||
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 << "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;
|
||||
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;
|
||||
<< (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;
|
||||
<< (final_estimate.S[0].rpy() * RAD_TO_DEG).transpose()
|
||||
<< "° | Truth: "
|
||||
<< (final_data.xi.S[0].rpy() * RAD_TO_DEG).transpose() << "°"
|
||||
<< std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Main function for the EqF demo
|
||||
* @param argc Number of arguments
|
||||
* @param argv Array of arguments
|
||||
* @return Exit code
|
||||
*/
|
||||
|
||||
|
||||
int main(int argc, char* argv[]) {
|
||||
std::cout << "ABC-EqF: Attitude-Bias-Calibration Equivariant Filter Demo" << std::endl;
|
||||
std::cout << "==============================================================" << std::endl;
|
||||
std::cout << "ABC-EqF: Attitude-Bias-Calibration Equivariant Filter Demo"
|
||||
<< std::endl;
|
||||
std::cout << "=============================================================="
|
||||
<< std::endl;
|
||||
|
||||
try {
|
||||
// Parse command line options
|
||||
|
|
@ -384,7 +392,8 @@ int main(int argc, char* argv[]) {
|
|||
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;
|
||||
std::cerr << "Usage: " << argv[0]
|
||||
<< " [csv_file_path] [max_rows] [downsample]" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
|
@ -399,7 +408,8 @@ int main(int argc, char* argv[]) {
|
|||
}
|
||||
|
||||
// Load data from CSV file
|
||||
std::vector<Data> data = loadDataFromCSV(csvFilePath, 0, maxRows, downsample);
|
||||
std::vector<Data> data =
|
||||
loadDataFromCSV(csvFilePath, 0, maxRows, downsample);
|
||||
|
||||
if (data.empty()) {
|
||||
std::cerr << "No data available to process. Exiting." << std::endl;
|
||||
|
|
@ -407,17 +417,19 @@ int main(int argc, char* argv[]) {
|
|||
}
|
||||
|
||||
// Initialize the EqF filter with one calibration state
|
||||
int n_cal = 1;
|
||||
int n_sensors = 2;
|
||||
|
||||
// Initial covariance - larger values allow faster convergence
|
||||
Matrix initialSigma = Matrix::Identity(6 + 3*n_cal, 6 + 3*n_cal);
|
||||
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
|
||||
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
|
||||
EqF filter(initialSigma, n_cal, n_sensors);
|
||||
EqFilter filter(initialSigma, n_sensors);
|
||||
|
||||
// Process data
|
||||
processDataWithEqF(filter, data);
|
||||
|
|
|
|||
Loading…
Reference in New Issue