1/**
2 * @file ABC.h
3 * @brief Core components for Attitude-Bias-Calibration systems
4 *
5 * This file contains fundamental components and utilities for the ABC system
6 * based on the paper "Overcoming Bias: Equivariant Filter Design for Biased
7 * Attitude Estimation with Online Calibration" by Fornasier et al.
8 * Authors: Darshan Rajasekaran & Jennifer Oum
9 */
10#ifndef ABC_H
11#define ABC_H
12#include <gtsam/base/Matrix.h>
13#include <gtsam/base/Vector.h>
14#include <gtsam/geometry/Rot3.h>
15#include <gtsam/geometry/Unit3.h>
16
17namespace gtsam {
18namespace abc_eqf_lib {
19using namespace std;
20using namespace gtsam;
21//========================================================================
22// Utility Functions
23//========================================================================
24
25//========================================================================
26// Utility Functions
27//========================================================================
28
29/// Check if a vector is a unit vector
30
31bool checkNorm(const Vector3& x, double tol = 1e-3);
32
33/// Check if vector contains NaN values
34
35bool hasNaN(const Vector3& vec);
36
37/// Create a block diagonal matrix from two matrices
38
39Matrix blockDiag(const Matrix& A, const Matrix& B);
40
41/// Repeat a block matrix n times along the diagonal
42
43Matrix repBlock(const Matrix& A, int n);
44
45// Utility Functions Implementation
46
47/**
48 * @brief Verifies if a vector has unit norm within tolerance
49 * @param x 3d vector
50 * @param tol optional tolerance
51 * @return Bool indicating that the vector norm is approximately 1
52 */
53bool checkNorm(const Vector3& x, double tol) {
54 return abs(x: x.norm() - 1) < tol || std::isnan(x: x.norm());
55}
56
57/**
58 * @brief Checks if the input vector has any NaNs
59 * @param vec A 3-D vector
60 * @return true if present, false otherwise
61 */
62bool hasNaN(const Vector3& vec) {
63 return std::isnan(x: vec[0]) || std::isnan(x: vec[1]) || std::isnan(x: vec[2]);
64}
65
66/**
67 * @brief Creates a block diagonal matrix from input matrices
68 * @param A Matrix A
69 * @param B Matrix B
70 * @return A single consolidated matrix with A in the top left and B in the
71 * bottom right
72 */
73Matrix blockDiag(const Matrix& A, const Matrix& B) {
74 if (A.size() == 0) {
75 return B;
76 } else if (B.size() == 0) {
77 return A;
78 } else {
79 Matrix result(A.rows() + B.rows(), A.cols() + B.cols());
80 result.setZero();
81 result.block(startRow: 0, startCol: 0, blockRows: A.rows(), blockCols: A.cols()) = A;
82 result.block(startRow: A.rows(), startCol: A.cols(), blockRows: B.rows(), blockCols: B.cols()) = B;
83 return result;
84 }
85}
86
87/**
88 * @brief Creates a block diagonal matrix by repeating a matrix 'n' times
89 * @param A A matrix
90 * @param n Number of times to be repeated
91 * @return Block diag matrix with A repeated 'n' times
92 */
93Matrix repBlock(const Matrix& A, int n) {
94 if (n <= 0) return Matrix();
95
96 Matrix result = A;
97 for (int i = 1; i < n; i++) {
98 result = blockDiag(A: result, B: A);
99 }
100 return result;
101}
102
103//========================================================================
104// Core Data Types
105//========================================================================
106
107/// Input struct for the Biased Attitude System
108
109struct Input {
110 Vector3 w; /// Angular velocity (3-vector)
111 Matrix Sigma; /// Noise covariance (6x6 matrix)
112 static Input random(); /// Random input
113 Matrix3 W() const { /// Return w as a skew symmetric matrix
114 return Rot3::Hat(xi: w);
115 }
116};
117
118/// Measurement struct
119struct Measurement {
120 Unit3 y; /// Measurement direction in sensor frame
121 Unit3 d; /// Known direction in global frame
122 Matrix3 Sigma; /// Covariance matrix of the measurement
123 int cal_idx = -1; /// Calibration index (-1 for calibrated sensor)
124};
125
126/// State class representing the state of the Biased Attitude System
127template <size_t N>
128class State {
129 public:
130 Rot3 R; // Attitude rotation matrix R
131 Vector3 b; // Gyroscope bias b
132 std::array<Rot3, N> S; // Sensor calibrations S
133
134 /// Constructor
135 State(const Rot3& R = Rot3::Identity(), const Vector3& b = Vector3::Zero(),
136 const std::array<Rot3, N>& S = std::array<Rot3, N>{})
137 : R(R), b(b), S(S) {}
138
139 /// Identity function
140 static State identity() {
141 std::array<Rot3, N> S_id{};
142 S_id.fill(Rot3::Identity());
143 return State(Rot3::Identity(), Vector3::Zero(), S_id);
144 }
145 /**
146 * Compute Local coordinates in the state relative to another state.
147 * @param other The other state
148 * @return Local coordinates in the tangent space
149 */
150 Vector localCoordinates(const State<N>& other) const {
151 Vector eps(6 + 3 * N);
152
153 // First 3 elements - attitude
154 eps.head<3>() = Rot3::Logmap(R: R.between(other.R));
155 // Next 3 elements - bias
156 // Next 3 elements - bias
157 eps.segment<3>(start: 3) = other.b - b;
158
159 // Remaining elements - calibrations
160 for (size_t i = 0; i < N; i++) {
161 eps.segment<3>(start: 6 + 3 * i) = Rot3::Logmap(R: S[i].between(other.S[i]));
162 }
163
164 return eps;
165 }
166
167 /**
168 * Retract from tangent space back to the manifold
169 * @param v Vector in the tangent space
170 * @return New state
171 */
172 State retract(const Vector& v) const {
173 if (v.size() != static_cast<Eigen::Index>(6 + 3 * N)) {
174 throw std::invalid_argument(
175 "Vector size does not match state dimensions");
176 }
177 Rot3 newR = R * Rot3::Expmap(v: v.head<3>());
178 Vector3 newB = b + v.segment<3>(start: 3);
179 std::array<Rot3, N> newS;
180 for (size_t i = 0; i < N; i++) {
181 newS[i] = S[i] * Rot3::Expmap(v: v.segment<3>(start: 6 + 3 * i));
182 }
183 return State(newR, newB, newS);
184 }
185};
186
187//========================================================================
188// Symmetry Group
189//========================================================================
190
191/**
192 * Symmetry group (SO(3) |x so(3)) x SO(3) x ... x SO(3)
193 * Each element of the B list is associated with a calibration state
194 */
195template <size_t N>
196struct G {
197 Rot3 A; /// First SO(3) element
198 Matrix3 a; /// so(3) element (skew-symmetric matrix)
199 std::array<Rot3, N> B; /// List of SO(3) elements for calibration
200
201 /// Initialize the symmetry group G
202 G(const Rot3& A = Rot3::Identity(), const Matrix3& a = Matrix3::Zero(),
203 const std::array<Rot3, N>& B = std::array<Rot3, N>{})
204 : A(A), a(a), B(B) {}
205
206 /// Group multiplication
207 G operator*(const G<N>& other) const {
208 std::array<Rot3, N> newB;
209 for (size_t i = 0; i < N; i++) {
210 newB[i] = B[i] * other.B[i];
211 }
212 return G(A * other.A, a + Rot3::Hat(xi: A.matrix() * Rot3::Vee(X: other.a)), newB);
213 }
214
215 /// Group inverse
216 G inv() const {
217 Matrix3 Ainv = A.inverse().matrix();
218 std::array<Rot3, N> Binv;
219 for (size_t i = 0; i < N; i++) {
220 Binv[i] = B[i].inverse();
221 }
222 return G(A.inverse(), -Rot3::Hat(xi: Ainv * Rot3::Vee(X: a)), Binv);
223 }
224
225 /// Identity element
226 static G identity(int n) {
227 std::array<Rot3, N> B;
228 B.fill(Rot3::Identity());
229 return G(Rot3::Identity(), Matrix3::Zero(), B);
230 }
231
232 /// Exponential map of the tangent space elements to the group
233 static G exp(const Vector& x) {
234 if (x.size() != static_cast<Eigen::Index>(6 + 3 * N)) {
235 throw std::invalid_argument("Vector size mismatch for group exponential");
236 }
237 Rot3 A = Rot3::Expmap(v: x.head<3>());
238 Vector3 a_vee = Rot3::ExpmapDerivative(x: -x.head<3>()) * x.segment<3>(start: 3);
239 Matrix3 a = Rot3::Hat(xi: a_vee);
240 std::array<Rot3, N> B;
241 for (size_t i = 0; i < N; i++) {
242 B[i] = Rot3::Expmap(v: x.segment<3>(start: 6 + 3 * i));
243 }
244 return G(A, a, B);
245 }
246};
247} // namespace abc_eqf_lib
248
249template <size_t N>
250struct traits<abc_eqf_lib::State<N>>
251 : internal::LieGroupTraits<abc_eqf_lib::State<N>> {};
252
253template <size_t N>
254struct traits<abc_eqf_lib::G<N>> : internal::LieGroupTraits<abc_eqf_lib::G<N>> {
255};
256
257} // namespace gtsam
258
259#endif // ABC_H
260