Implement ABC_EQF in cpp - Inputs are simulated- Use with caution
darshan-17
jenniferoum
parent
3a1188a2ee
commit
673336545b
|
|
@ -0,0 +1,13 @@
|
|||
add_executable(ABC_EqF
|
||||
main.cpp
|
||||
EqF.cpp
|
||||
State.cpp
|
||||
Input.cpp
|
||||
G.cpp
|
||||
Direction.cpp
|
||||
Measurements.cpp
|
||||
utilities.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(ABC_EqF gtsam)
|
||||
|
||||
|
|
@ -0,0 +1,41 @@
|
|||
//
|
||||
// Created by darshan on 3/11/25.
|
||||
//
|
||||
|
||||
#ifndef DATA_H
|
||||
#define DATA_H
|
||||
//#pragma once
|
||||
|
||||
#include "State.h"
|
||||
#include "Input.h"
|
||||
#include "Measurements.h"
|
||||
#include <vector>
|
||||
|
||||
/**
|
||||
* 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
|
||||
|
||||
/**
|
||||
* Initialize Data
|
||||
* @param xi Ground-truth state
|
||||
* @param n_cal Number of calibration states
|
||||
* @param u Input measurements
|
||||
* @param y Output measurements
|
||||
* @param n_meas Number of measurements
|
||||
* @param t Time
|
||||
* @param dt Time step
|
||||
*/
|
||||
Data(const State& xi, int n_cal, const Input& u,
|
||||
const std::vector<Measurement>& y, int n_meas,
|
||||
double t, double dt)
|
||||
: xi(xi), n_cal(n_cal), u(u), y(y), n_meas(n_meas), t(t), dt(dt) {}
|
||||
};
|
||||
#endif //DATA_H
|
||||
|
|
@ -0,0 +1,13 @@
|
|||
//
|
||||
// Created by darshan on 3/11/25.
|
||||
//
|
||||
|
||||
#include "Direction.h"
|
||||
#include "utilities.h"
|
||||
#include <stdexcept>
|
||||
|
||||
Direction::Direction(const Vector3& d_vec) : d(d_vec) {
|
||||
if (!checkNorm(d_vec)) {
|
||||
throw std::invalid_argument("Direction must be a unit vector");
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,27 @@
|
|||
//
|
||||
// Created by darshan on 3/11/25.
|
||||
//
|
||||
|
||||
#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);
|
||||
};
|
||||
#endif //DIRECTION_H
|
||||
|
|
@ -0,0 +1,258 @@
|
|||
//
|
||||
// Created by darshan on 3/11/25.
|
||||
//
|
||||
#include "EqF.h"
|
||||
#include "utilities.h"
|
||||
#include <Eigen/Dense>
|
||||
#include <stdexcept>
|
||||
#include <functional>
|
||||
|
||||
// Implementation of helper functions
|
||||
|
||||
Vector lift(const State& xi, const Input& u) {
|
||||
int n = xi.S.size();
|
||||
Vector L = Vector::Zero(6 + 3 * n);
|
||||
|
||||
// First 3 elements
|
||||
L.head<3>() = u.w - xi.b;
|
||||
|
||||
// Next 3 elements
|
||||
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>();
|
||||
}
|
||||
|
||||
return L;
|
||||
}
|
||||
|
||||
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");
|
||||
}
|
||||
|
||||
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 - vee(X.a)),
|
||||
new_S);
|
||||
}
|
||||
|
||||
Input velocityAction(const G& X, const Input& u) {
|
||||
return Input(X.A.inverse().matrix() * (u.w - vee(X.a)), u.Sigma);
|
||||
}
|
||||
|
||||
Vector3 outputAction(const G& X, const Direction& y, int idx) {
|
||||
if (idx == -1) {
|
||||
return X.A.inverse().matrix() * y.d.unitVector();
|
||||
} else {
|
||||
if (idx >= static_cast<int>(X.B.size())) {
|
||||
throw std::out_of_range("Calibration index out of range");
|
||||
}
|
||||
return X.B[idx].inverse().matrix() * y.d.unitVector();
|
||||
}
|
||||
}
|
||||
|
||||
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");
|
||||
}
|
||||
}
|
||||
|
||||
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 stateActionDiff(const State& xi) {
|
||||
std::function<Vector(const Vector&)> coordsAction =
|
||||
[&xi](const Vector& U) {
|
||||
return local_coords(stateAction(G::exp(U), xi));
|
||||
};
|
||||
|
||||
Vector zeros = Vector::Zero(6 + 3 * xi.S.size());
|
||||
Matrix differential = numericalDifferential(coordsAction, zeros);
|
||||
return differential;
|
||||
}
|
||||
|
||||
// EqF class implementation
|
||||
|
||||
EqF::EqF(const Matrix& Sigma, int n, int m)
|
||||
: __dof(6 + 3 * n), __n_cal(n), __n_sensor(m), __X_hat(G::identity(n)),
|
||||
__Sigma(Sigma), __xi_0(State::identity(n)) {
|
||||
|
||||
if (Sigma.rows() != __dof || Sigma.cols() != __dof) {
|
||||
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");
|
||||
}
|
||||
|
||||
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");
|
||||
}
|
||||
|
||||
// Compute differential of phi
|
||||
__Dphi0 = stateActionDiff(__xi_0);
|
||||
__InnovationLift = __Dphi0.completeOrthogonalDecomposition().pseudoInverse();
|
||||
}
|
||||
|
||||
State EqF::stateEstimate() const {
|
||||
return stateAction(__X_hat, __xi_0);
|
||||
}
|
||||
|
||||
void EqF::propagation(const Input& u, double dt) {
|
||||
State 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 M_DT = (Bt * tempSigma * Bt.transpose()) * dt;
|
||||
|
||||
__X_hat = __X_hat * G::exp(L * dt);
|
||||
__Sigma = Phi_DT * __Sigma * Phi_DT.transpose() + M_DT;
|
||||
}
|
||||
|
||||
void EqF::update(const Measurement& y) {
|
||||
if (y.cal_idx > __n_cal) {
|
||||
throw std::invalid_argument("Calibration index out of range");
|
||||
}
|
||||
|
||||
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 EqF::__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>(3, 3) = W0;
|
||||
Matrix A2 = repBlock(W0, __n_cal);
|
||||
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");
|
||||
}
|
||||
}
|
||||
|
||||
Matrix EqF::__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;
|
||||
|
||||
if (COORDINATE == "EXPONENTIAL") {
|
||||
Phi1.block<3, 3>(0, 0) = Matrix3::Identity();
|
||||
Phi1.block<3, 3>(0, 3) = Phi12;
|
||||
Phi1.block<3, 3>(3, 3) = Phi22;
|
||||
Matrix Phi2 = repBlock(Phi22, __n_cal);
|
||||
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");
|
||||
}
|
||||
}
|
||||
|
||||
Matrix EqF::__inputMatrixBt() const {
|
||||
if (COORDINATE == "EXPONENTIAL") {
|
||||
Matrix B1 = blockDiag(__X_hat.A.matrix(), __X_hat.A.matrix());
|
||||
Matrix B2;
|
||||
|
||||
for (const auto& B : __X_hat.B) {
|
||||
if (B2.size() == 0) {
|
||||
B2 = B.matrix();
|
||||
} else {
|
||||
B2 = blockDiag(B2, B.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");
|
||||
}
|
||||
}
|
||||
|
||||
Matrix EqF::__measurementMatrixC(const Direction& d, int idx) const {
|
||||
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());
|
||||
}
|
||||
|
||||
Matrix3 wedge_d = wedge(d.d.unitVector());
|
||||
Matrix result(3, __dof);
|
||||
result.block<3, 3>(0, 0) = wedge_d;
|
||||
result.block<3, 3>(0, 3) = Matrix3::Zero();
|
||||
result.block(0, 6, 3, 3 * __n_cal) = Cc;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
Matrix EqF::__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())) {
|
||||
throw std::out_of_range("Calibration index out of range");
|
||||
}
|
||||
return __X_hat.B[idx].matrix();
|
||||
} else {
|
||||
return __X_hat.A.matrix();
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,153 @@
|
|||
//
|
||||
// Created by darshan on 3/11/25.
|
||||
//
|
||||
|
||||
#ifndef EQF_H
|
||||
#define EQF_H
|
||||
#pragma once
|
||||
|
||||
#include "State.h"
|
||||
#include "Input.h"
|
||||
#include "G.h"
|
||||
#include "Direction.h"
|
||||
#include "Measurements.h"
|
||||
#include <gtsam/base/Matrix.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
/**
|
||||
* Equivariant Filter (EqF) implementation
|
||||
*/
|
||||
class EqF {
|
||||
private:
|
||||
int __dof; // Degrees of freedom
|
||||
int __n_cal; // Number of calibration states
|
||||
int __n_sensor; // Number of sensors
|
||||
G __X_hat; // Filter state
|
||||
Matrix __Sigma; // Error covariance
|
||||
State __xi_0; // Origin state
|
||||
Matrix __Dphi0; // Differential of phi at origin
|
||||
Matrix __InnovationLift; // Innovation lift matrix
|
||||
|
||||
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);
|
||||
|
||||
/**
|
||||
* Return estimated state
|
||||
* @return Current state estimate
|
||||
*/
|
||||
State stateEstimate() const;
|
||||
|
||||
/**
|
||||
* Propagate the filter state
|
||||
* @param u Angular velocity measurement
|
||||
* @param dt Time step
|
||||
*/
|
||||
void propagation(const Input& u, double dt);
|
||||
|
||||
/**
|
||||
* Update the filter state with a measurement
|
||||
* @param y Direction measurement
|
||||
*/
|
||||
void update(const Measurement& y);
|
||||
|
||||
private:
|
||||
/**
|
||||
* Return the state matrix A0t (Equation 14a)
|
||||
* @param u Input
|
||||
* @return State matrix A0t
|
||||
*/
|
||||
Matrix __stateMatrixA(const Input& u) const;
|
||||
|
||||
/**
|
||||
* Return the state transition matrix Phi (Equation 17)
|
||||
* @param u Input
|
||||
* @param dt Time step
|
||||
* @return State transition matrix Phi
|
||||
*/
|
||||
Matrix __stateTransitionMatrix(const Input& u, double dt) const;
|
||||
|
||||
/**
|
||||
* Return the Input matrix Bt
|
||||
* @return Input matrix Bt
|
||||
*/
|
||||
Matrix __inputMatrixBt() const;
|
||||
|
||||
/**
|
||||
* Return the measurement matrix C0 (Equation 14b)
|
||||
* @param d Known direction
|
||||
* @param idx Calibration index
|
||||
* @return Measurement matrix C0
|
||||
*/
|
||||
Matrix __measurementMatrixC(const Direction& d, int idx) const;
|
||||
|
||||
/**
|
||||
* Return the measurement output matrix Dt
|
||||
* @param idx Calibration index
|
||||
* @return Measurement output matrix Dt
|
||||
*/
|
||||
Matrix __outputMatrixDt(int idx) const;
|
||||
};
|
||||
|
||||
// Function declarations for helper functions used by EqF
|
||||
|
||||
/**
|
||||
* 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);
|
||||
|
||||
/**
|
||||
* Action of the symmetry group on the state space (Equation 4)
|
||||
* @param X Group element
|
||||
* @param xi State
|
||||
* @return New state after group action
|
||||
*/
|
||||
State stateAction(const G& X, const State& xi);
|
||||
|
||||
/**
|
||||
* Action of the symmetry group on the input space (Equation 5)
|
||||
* @param X Group element
|
||||
* @param u Input
|
||||
* @return New input after group action
|
||||
*/
|
||||
Input velocityAction(const G& X, const Input& u);
|
||||
|
||||
/**
|
||||
* Action of the symmetry group on the output space (Equation 6)
|
||||
* @param X Group element
|
||||
* @param y Direction measurement
|
||||
* @param idx Calibration index
|
||||
* @return New direction after group action
|
||||
*/
|
||||
Vector3 outputAction(const G& X, const Direction& y, int idx = -1);
|
||||
|
||||
/**
|
||||
* Local coordinates assuming xi_0 = identity (Equation 9)
|
||||
* @param e State representing equivariant error
|
||||
* @return Local coordinates
|
||||
*/
|
||||
Vector local_coords(const State& e);
|
||||
|
||||
/**
|
||||
* Local coordinates inverse assuming xi_0 = identity
|
||||
* @param eps Local coordinates
|
||||
* @return Corresponding state
|
||||
*/
|
||||
State local_coords_inv(const Vector& eps);
|
||||
|
||||
/**
|
||||
* Differential of the phi action at E = Id in local coordinates
|
||||
* @param xi State
|
||||
* @return Differential matrix
|
||||
*/
|
||||
Matrix stateActionDiff(const State& xi);
|
||||
#endif //EQF_H
|
||||
|
|
@ -0,0 +1,61 @@
|
|||
//
|
||||
// Created by darshan on 3/11/25.
|
||||
//
|
||||
#include "G.h"
|
||||
#include "utilities.h"
|
||||
#include <stdexcept>
|
||||
|
||||
G::G(const Rot3& A, const Matrix3& a, const std::vector<Rot3>& B)
|
||||
: A(A), a(a), B(B) {}
|
||||
|
||||
G 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 + wedge(A.matrix() * vee(other.a)),
|
||||
new_B);
|
||||
}
|
||||
|
||||
G 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(),
|
||||
-wedge(A_inv * vee(a)),
|
||||
B_inv);
|
||||
}
|
||||
|
||||
G G::identity(int n) {
|
||||
std::vector<Rot3> B(n, Rot3::Identity());
|
||||
return G(Rot3::Identity(), Matrix3::Zero(), B);
|
||||
}
|
||||
|
||||
G 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 = Rot3LeftJacobian(x.head<3>()) * x.segment<3>(3);
|
||||
Matrix3 a = wedge(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);
|
||||
}
|
||||
|
|
@ -0,0 +1,63 @@
|
|||
//
|
||||
// Created by darshan on 3/11/25.
|
||||
//
|
||||
|
||||
#ifndef G_H
|
||||
#define G_H
|
||||
|
||||
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <vector>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
/**
|
||||
* 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
|
||||
* @param A SO3 element
|
||||
* @param a so(3) element (skew symmetric matrix)
|
||||
* @param B list of SO3 elements
|
||||
*/
|
||||
G(const Rot3& A = Rot3::Identity(),
|
||||
const Matrix3& a = Matrix3::Zero(),
|
||||
const std::vector<Rot3>& B = std::vector<Rot3>());
|
||||
|
||||
/**
|
||||
* Define the group operation (multiplication)
|
||||
* @param other Another group element
|
||||
* @return The product of this and other
|
||||
*/
|
||||
G operator*(const G& other) const;
|
||||
|
||||
/**
|
||||
* Return the inverse element of the symmetry group
|
||||
* @return The inverse of this group element
|
||||
*/
|
||||
G inv() const;
|
||||
|
||||
/**
|
||||
* Return the identity of the symmetry group
|
||||
* @param n Number of calibration elements
|
||||
* @return The identity element with n calibration components
|
||||
*/
|
||||
static G identity(int n);
|
||||
|
||||
/**
|
||||
* Return a group element X given by X = exp(x)
|
||||
* @param x Vector representation of Lie algebra element
|
||||
* @return Group element given by the exponential of x
|
||||
*/
|
||||
static G exp(const Vector& x);
|
||||
};
|
||||
#endif //G_H
|
||||
|
|
@ -0,0 +1,29 @@
|
|||
//
|
||||
// Created by darshan on 3/11/25.
|
||||
//
|
||||
#include "Input.h"
|
||||
#include "utilities.h"
|
||||
#include <Eigen/Dense>
|
||||
#include <stdexcept>
|
||||
|
||||
Input::Input(const Vector3& w, const Matrix& Sigma)
|
||||
: w(w), Sigma(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");
|
||||
}
|
||||
}
|
||||
|
||||
Matrix3 Input::W() const {
|
||||
return wedge(w);
|
||||
}
|
||||
|
||||
Input Input::random() {
|
||||
Vector3 w = Vector3::Random();
|
||||
return Input(w, Matrix::Identity(6, 6));
|
||||
}
|
||||
|
|
@ -0,0 +1,41 @@
|
|||
//
|
||||
// Created by darshan on 3/11/25.
|
||||
//
|
||||
|
||||
#ifndef INPUT_H
|
||||
#define INPUT_H
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
/**
|
||||
* Input class for the Biased Attitude System
|
||||
*/
|
||||
class Input {
|
||||
public:
|
||||
Vector3 w; // Angular velocity
|
||||
Matrix Sigma; // Noise covariance
|
||||
|
||||
/**
|
||||
* Initialize Input
|
||||
* @param w Angular velocity (3-vector)
|
||||
* @param Sigma Noise covariance (6x6 matrix)
|
||||
*/
|
||||
Input(const Vector3& w, const Matrix& Sigma);
|
||||
|
||||
/**
|
||||
* Return the Input as a skew-symmetric matrix
|
||||
* @return w as a skew-symmetric matrix
|
||||
*/
|
||||
Matrix3 W() const;
|
||||
|
||||
/**
|
||||
* Return a random angular velocity
|
||||
* @return A random angular velocity as Input element
|
||||
*/
|
||||
static Input random();
|
||||
};
|
||||
|
||||
#endif //INPUT_H
|
||||
|
|
@ -0,0 +1,17 @@
|
|||
//
|
||||
// Created by darshan on 3/11/25.
|
||||
//
|
||||
#include "Measurements.h"
|
||||
#include <Eigen/Dense>
|
||||
#include <stdexcept>
|
||||
|
||||
Measurement::Measurement(const Vector3& y_vec, const Vector3& d_vec,
|
||||
const Matrix3& Sigma, int i)
|
||||
: y(y_vec), d(d_vec), Sigma(Sigma), cal_idx(i) {
|
||||
|
||||
// Check positive semi-definite
|
||||
Eigen::SelfAdjointEigenSolver<Matrix3> eigensolver(Sigma);
|
||||
if (eigensolver.eigenvalues().minCoeff() < -1e-10) {
|
||||
throw std::invalid_argument("Covariance matrix must be semi-positive definite");
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
//
|
||||
// Created by darshan on 3/11/25.
|
||||
//
|
||||
|
||||
#ifndef MEASUREMENTS_H
|
||||
#define MEASUREMENTS_H
|
||||
|
||||
|
||||
#include "Direction.h"
|
||||
#include <gtsam/base/Matrix.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
/**
|
||||
* Measurement class
|
||||
* cal_idx is an index corresponding to the calibration related to the measurement
|
||||
* cal_idx = -1 indicates the measurement is from a calibrated sensor
|
||||
*/
|
||||
class Measurement {
|
||||
public:
|
||||
Direction y; // Measurement direction in sensor frame
|
||||
Direction d; // Known direction in global frame
|
||||
Matrix3 Sigma; // Covariance matrix of the measurement
|
||||
int cal_idx = -1; // Calibration index (-1 for calibrated sensor)
|
||||
|
||||
/**
|
||||
* Initialize measurement
|
||||
* @param y_vec Direction measurement in sensor frame
|
||||
* @param d_vec Known direction in global frame
|
||||
* @param Sigma Measurement noise covariance
|
||||
* @param i Calibration index (-1 for calibrated sensor)
|
||||
*/
|
||||
Measurement(const Vector3& y_vec, const Vector3& d_vec,
|
||||
const Matrix3& Sigma, int i = -1);
|
||||
};
|
||||
#endif //MEASUREMENTS_H
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
//
|
||||
// Created by darshan on 3/11/25.
|
||||
//
|
||||
#include "State.h"
|
||||
|
||||
State::State(const Rot3& R, const Vector3& b, const std::vector<Rot3>& S)
|
||||
: R(R), b(b), S(S) {}
|
||||
|
||||
State State::identity(int n) {
|
||||
std::vector<Rot3> calibrations(n, Rot3::Identity());
|
||||
return State(Rot3::Identity(), Vector3::Zero(), calibrations);
|
||||
}
|
||||
|
|
@ -0,0 +1,29 @@
|
|||
//
|
||||
// Created by darshan on 3/11/25.
|
||||
//
|
||||
|
||||
#ifndef STATE_H
|
||||
#define STATE_H
|
||||
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <vector>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
/**
|
||||
* 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
|
||||
|
||||
State(const Rot3& R = Rot3::Identity(),
|
||||
const Vector3& b = Vector3::Zero(),
|
||||
const std::vector<Rot3>& S = std::vector<Rot3>());
|
||||
|
||||
static State identity(int n);
|
||||
};
|
||||
#endif //STATE_H
|
||||
|
|
@ -0,0 +1,195 @@
|
|||
//
|
||||
// Created by darshan on 3/11/25.
|
||||
//
|
||||
#include "EqF.h"
|
||||
#include "State.h"
|
||||
#include "Input.h"
|
||||
#include "Direction.h"
|
||||
#include "Measurements.h"
|
||||
#include "Data.h"
|
||||
#include "utilities.h"
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// Simplified data loading function - in a real application, implement proper CSV parsing
|
||||
std::vector<Data> loadSimulatedData() {
|
||||
std::vector<Data> data_list;
|
||||
|
||||
double t = 0.0;
|
||||
double dt = 0.01;
|
||||
|
||||
// Number of data points
|
||||
int num_points = 100;
|
||||
|
||||
// Set up one calibration state
|
||||
int n_cal = 1;
|
||||
|
||||
for (int i = 0; i < num_points; i++) {
|
||||
t += dt;
|
||||
|
||||
// Create a simple sinusoidal trajectory
|
||||
double angle = 0.1 * sin(t);
|
||||
Rot3 R = Rot3::Rz(angle);
|
||||
|
||||
// Create a bias
|
||||
Vector3 b(0.01, 0.02, 0.03);
|
||||
|
||||
// Create a calibration
|
||||
std::vector<Rot3> S;
|
||||
S.push_back(Rot3::Ry(0.05));
|
||||
|
||||
// State
|
||||
State xi(R, b, S);
|
||||
|
||||
// Input (angular velocity)
|
||||
Vector3 w(0.1 * cos(t), 0.05 * sin(t), 0.02);
|
||||
Matrix Sigma_u = Matrix::Identity(6, 6) * 0.01;
|
||||
Input u(w, Sigma_u);
|
||||
|
||||
// Measurements
|
||||
std::vector<Measurement> measurements;
|
||||
|
||||
// Measurement 1 - from uncalibrated sensor
|
||||
Vector3 d1_vec = Vector3(1, 0, 0).normalized(); // Known direction in global frame
|
||||
Vector3 y1_vec = S[0].inverse().matrix() * R.inverse().matrix() * d1_vec; // Direction in sensor frame
|
||||
Matrix3 Sigma1 = Matrix3::Identity() * 0.01;
|
||||
measurements.push_back(Measurement(y1_vec, d1_vec, Sigma1, 0)); // cal_idx = 0
|
||||
|
||||
// Measurement 2 - from calibrated sensor
|
||||
Vector3 d2_vec = Vector3(0, 1, 0).normalized(); // Known direction in global frame
|
||||
Vector3 y2_vec = R.inverse().matrix() * d2_vec; // Direction in sensor frame
|
||||
Matrix3 Sigma2 = Matrix3::Identity() * 0.01;
|
||||
measurements.push_back(Measurement(y2_vec, d2_vec, Sigma2, -1)); // cal_idx = -1
|
||||
|
||||
// Add to data list
|
||||
data_list.push_back(Data(xi, n_cal, u, measurements, 2, t, dt));
|
||||
}
|
||||
|
||||
return data_list;
|
||||
}
|
||||
|
||||
void runSimulation(EqF& filter, const std::vector<Data>& data) {
|
||||
std::cout << "Starting simulation with " << data.size() << " data points..." << std::endl;
|
||||
|
||||
// Track time for performance measurement
|
||||
auto start = std::chrono::high_resolution_clock::now();
|
||||
|
||||
// Store results for analysis
|
||||
std::vector<double> times;
|
||||
std::vector<Vector3> attitude_errors;
|
||||
std::vector<Vector3> bias_errors;
|
||||
std::vector<Vector3> calibration_errors;
|
||||
|
||||
for (const auto& d : data) {
|
||||
// Propagation
|
||||
try {
|
||||
filter.propagation(d.u, d.dt);
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "Propagation error at t=" << d.t << ": " << e.what() << std::endl;
|
||||
continue;
|
||||
}
|
||||
|
||||
// Update with measurements
|
||||
for (const auto& y : d.y) {
|
||||
try {
|
||||
if (!std::isnan(y.y.d.unitVector().norm()) && !std::isnan(y.d.d.unitVector().norm())) {
|
||||
filter.update(y);
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "Update error at t=" << d.t << ": " << e.what() << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
// Get state estimate
|
||||
State estimate = filter.stateEstimate();
|
||||
|
||||
// Compute errors
|
||||
Vector3 att_error = Rot3::Logmap(d.xi.R.between(estimate.R));
|
||||
Vector3 bias_error = estimate.b - d.xi.b;
|
||||
Vector3 cal_error = Vector3::Zero();
|
||||
if (!d.xi.S.empty() && !estimate.S.empty()) {
|
||||
cal_error = Rot3::Logmap(d.xi.S[0].between(estimate.S[0]));
|
||||
}
|
||||
|
||||
// Store results
|
||||
times.push_back(d.t);
|
||||
attitude_errors.push_back(att_error);
|
||||
bias_errors.push_back(bias_error);
|
||||
calibration_errors.push_back(cal_error);
|
||||
|
||||
// Print some info
|
||||
if (d.t < 0.1 || fmod(d.t, 1.0) < d.dt) {
|
||||
std::cout << "Time: " << d.t
|
||||
<< ", Attitude error (deg): " << (att_error.norm() * 180.0/M_PI)
|
||||
<< ", Bias error: " << bias_error.norm()
|
||||
<< ", Calibration error (deg): " << (cal_error.norm() * 180.0/M_PI)
|
||||
<< std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
auto end = std::chrono::high_resolution_clock::now();
|
||||
std::chrono::duration<double> elapsed = end - start;
|
||||
std::cout << "Simulation completed in " << elapsed.count() << " seconds" << std::endl;
|
||||
|
||||
// Print summary statistics
|
||||
double avg_att_error = 0.0;
|
||||
double avg_bias_error = 0.0;
|
||||
double avg_cal_error = 0.0;
|
||||
|
||||
for (size_t i = 0; i < times.size(); i++) {
|
||||
avg_att_error += attitude_errors[i].norm();
|
||||
avg_bias_error += bias_errors[i].norm();
|
||||
avg_cal_error += calibration_errors[i].norm();
|
||||
}
|
||||
|
||||
avg_att_error /= times.size();
|
||||
avg_bias_error /= times.size();
|
||||
avg_cal_error /= times.size();
|
||||
|
||||
std::cout << "Average attitude error (deg): " << (avg_att_error * 180.0/M_PI) << std::endl;
|
||||
std::cout << "Average bias error: " << avg_bias_error << std::endl;
|
||||
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;
|
||||
}
|
||||
|
|
@ -0,0 +1,87 @@
|
|||
//
|
||||
// Created by darshan on 3/11/25.
|
||||
//
|
||||
#include "utilities.h"
|
||||
#include <cmath>
|
||||
|
||||
// Global configuration
|
||||
const std::string COORDINATE = "EXPONENTIAL"; // Alternative: "NORMAL"
|
||||
|
||||
Matrix3 wedge(const Vector3& vec) {
|
||||
Matrix3 mat;
|
||||
mat << 0.0, -vec(2), vec(1),
|
||||
vec(2), 0.0, -vec(0),
|
||||
-vec(1), vec(0), 0.0;
|
||||
return mat;
|
||||
}
|
||||
|
||||
Vector3 vee(const Matrix3& mat) {
|
||||
Vector3 vec;
|
||||
vec << mat(2, 1), mat(0, 2), mat(1, 0);
|
||||
return vec;
|
||||
}
|
||||
|
||||
Matrix3 Rot3LeftJacobian(const Vector3& arr) {
|
||||
double angle = arr.norm();
|
||||
|
||||
// Near |phi|==0, use first order Taylor expansion
|
||||
if (angle < 1e-10) {
|
||||
return Matrix3::Identity() + 0.5 * wedge(arr);
|
||||
}
|
||||
|
||||
Vector3 axis = arr / angle;
|
||||
double s = sin(angle);
|
||||
double c = cos(angle);
|
||||
|
||||
return (s / angle) * Matrix3::Identity() +
|
||||
(1 - (s / angle)) * (axis * axis.transpose()) +
|
||||
((1 - c) / angle) * wedge(axis);
|
||||
}
|
||||
|
||||
bool checkNorm(const Vector3& x, double tol) {
|
||||
return abs(x.norm() - 1) < tol || std::isnan(x.norm());
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
|
@ -0,0 +1,30 @@
|
|||
//
|
||||
// Created by darshan on 3/11/25.
|
||||
//
|
||||
|
||||
#ifndef UTILITIES_H
|
||||
#define UTILITIES_H
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <Eigen/Dense>
|
||||
#include <functional>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
// Global configuration
|
||||
extern const std::string COORDINATE; // "EXPONENTIAL" or "NORMAL"
|
||||
|
||||
/**
|
||||
* Utility functions
|
||||
*/
|
||||
Matrix3 wedge(const Vector3& vec);
|
||||
Vector3 vee(const Matrix3& mat);
|
||||
Matrix3 Rot3LeftJacobian(const Vector3& arr);
|
||||
bool checkNorm(const Vector3& x, double tol = 1e-3);
|
||||
Matrix blockDiag(const Matrix& A, const Matrix& B);
|
||||
Matrix repBlock(const Matrix& A, int n);
|
||||
Matrix numericalDifferential(std::function<Vector(const Vector&)> f, const Vector& x);
|
||||
#endif //UTILITIES_H
|
||||
|
|
@ -17,3 +17,4 @@ if (NOT GTSAM_USE_BOOST_FEATURES)
|
|||
endif()
|
||||
|
||||
gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;${Boost_PROGRAM_OPTIONS_LIBRARY}" ${GTSAM_BUILD_EXAMPLES_ALWAYS})
|
||||
add_subdirectory(ABC_EQF)
|
||||
Loading…
Reference in New Issue