LOST 0.0.1
LOST: Open-source Star Tracker
Loading...
Searching...
No Matches
attitude-estimators.cpp
Go to the documentation of this file.
2
3#include <eigen3/Eigen/Dense>
4#include <eigen3/Eigen/Eigenvalues>
5
6#include "decimal.hpp"
7
8namespace lost {
9
10#define EPSILON DECIMAL(0.0001) // threshold for 0 for Newton-Raphson method
11#define QUEST_MAX_ITERATIONS 20 // stops Newton-Raphson from looping forever if it never settles (happens in float mode)
12
14 const Stars &stars,
15 const Catalog &catalog,
16 const StarIdentifiers &starIdentifiers) {
17 if (starIdentifiers.size() < 2) {
18 return Attitude();
19 }
20 assert(stars.size() >= 2);
21
22 // attitude profile matrix
23 #ifdef LOST_FLOAT_MODE
24 Eigen::Matrix3f B;
25 #else
26 Eigen::Matrix3d B;
27 #endif
28
29
30 B.setZero();
31 for (const StarIdentifier &s : starIdentifiers) {
32 Star bStar = stars[s.starIndex];
33 Vec3 bStarSpatial = camera.CameraToSpatial(bStar.position);
34
35 #ifdef LOST_FLOAT_MODE
36 Eigen::Vector3f bi;
37 Eigen::Vector3f ri;
38 #else
39 Eigen::Vector3d bi;
40 Eigen::Vector3d ri;
41 #endif
42
43 bi << bStarSpatial.x, bStarSpatial.y, bStarSpatial.z;
44
45 CatalogStar rStar = catalog[s.catalogIndex];
46 ri << rStar.spatial.x, rStar.spatial.y, rStar.spatial.z;
47 //Weight = 1 (can be changed later, in which case we want to make a vector to hold all weights {ai})
48 // Calculate matrix B = sum({ai}{bi}{ri}T)
49 B += ri * bi.transpose() * s.weight;
50 }
51
52 // S = B + Transpose(B)
53 #ifdef LOST_FLOAT_MODE
54 Eigen::Matrix3f S = B + B.transpose();
55 #else
56 Eigen::Matrix3d S = B + B.transpose();
57 #endif
58
59 //sigma = B[0][0] + B[1][1] + B[2][2]
60 decimal sigma = B.trace();
61
62 //Z = [[B[1][2] - B[2][1]], [B[2][0] - B[0][2]], [B[0][1] - B[1][0]]]
63 #ifdef LOST_FLOAT_MODE
64 Eigen::Vector3f Z;
65 #else
66 Eigen::Vector3d Z;
67 #endif
68
69 Z << B(1,2) - B(2,1),
70 B(2,0) - B(0,2),
71 B(0,1) - B(1,0);
72
73 // K = [[[sigma], [Z[0]], [Z[1]], [Z[2]]], [[Z[0]], [S[0][0] - sigma], [S[0][1]], [S[0][2]]], [[Z[1]], [S[1][0]], [S[1][1] - sigma], [S[1][2]]], [[Z[2]], [S[2][0]], [S[2][1]], [S[2][2] - sigma]]]
74 #ifdef LOST_FLOAT_MODE
75 Eigen::Matrix4f K;
76 #else
77 Eigen::Matrix4d K;
78 #endif
79
80 K << sigma, Z(0), Z(1), Z(2),
81 Z(0), S(0,0) - sigma, S(0,1), S(0,2),
82 Z(1), S(1,0), S(1,1) - sigma, S(1,2),
83 Z(2), S(2,0), S(2,1), S(2,2) - sigma;
84
85 //Find eigenvalues of K, store the largest one as lambda
86 //find the maximum index
87 #ifdef LOST_FLOAT_MODE
88 Eigen::EigenSolver<Eigen::Matrix4f> solver(K);
89 Eigen::Vector4cf values = solver.eigenvalues();
90 Eigen::Matrix4cf vectors = solver.eigenvectors();
91 #else
92 Eigen::EigenSolver<Eigen::Matrix4d> solver(K);
93 Eigen::Vector4cd values = solver.eigenvalues();
94 Eigen::Matrix4cd vectors = solver.eigenvectors();
95 #endif
96
97 int maxIndex = 0;
98 decimal maxEigenvalue = values(0).real();
99 for (int i = 1; i < values.size(); i++) {
100 if (values(i).real() > maxEigenvalue) {
101 maxIndex = i;
102 maxEigenvalue = values(i).real();
103 }
104 }
105
106 //The return quaternion components = eigenvector assocaited with lambda
107 #ifdef LOST_FLOAT_MODE
108 Eigen::Vector4cf maxEigenvector = vectors.col(maxIndex);
109 #else
110 Eigen::Vector4cd maxEigenvector = vectors.col(maxIndex);
111 #endif
112
113 // IMPORTANT: The matrix K is symmetric -- clearly first row and first column are equal.
114 // Furthermore, S is symmetric because s_i,j = b_i,j + b_j,i and s_j,i=b_j,i + b_i,j, so the
115 // bottom right 3x3 block of K is symmetric too. Thus all its eigenvalues and eigenvectors
116 // are real.
117 return Attitude(Quaternion(maxEigenvector[0].real(),
118 maxEigenvector[1].real(),
119 maxEigenvector[2].real(),
120 maxEigenvector[3].real()));
121}
122
124static Mat3 TriadCoordinateFrame(Vec3 v1, Vec3 v2) {
125 Vec3 d1 = v1.Normalize();
126 Vec3 d2 = v1.CrossProduct(v2).Normalize();
127 Vec3 d3 = d1.CrossProduct(d2).Normalize();
128 return {
129 d1.x, d2.x, d3.x,
130 d1.y, d2.y, d3.y,
131 d1.z, d2.z, d3.z,
132 };
133}
134
136 const Stars &stars,
137 const Catalog &catalog,
138 const StarIdentifiers &starIds) {
139 if (starIds.size() < 2) {
140 return Attitude();
141 }
142 assert(stars.size() >= 2);
143
144 // TODO: Better way of picking the two stars
146 a = starIds[0],
147 b = starIds[starIds.size()/2];
148
149 Mat3 photoFrame = TriadCoordinateFrame(camera.CameraToSpatial(stars[a.starIndex].position),
150 camera.CameraToSpatial(stars[b.starIndex].position));
151 Mat3 catalogFrame = TriadCoordinateFrame(catalog[a.catalogIndex].spatial,
152 catalog[b.catalogIndex].spatial);
153
154 // Now, we calculate the rotation that would bring the catalog vector to the photo vector. We
155 // have the relationship photoFrame = attitude*catalogFrame, and since catalogFrame is an
156 // orthogonal matrix, we have attitude = photoFrame*transpase(catalogFrame)
157 return Attitude(photoFrame*catalogFrame.Transpose());
158}
159
164decimal QuestCharPoly(decimal x, decimal a, decimal b, decimal c, decimal d, decimal s) {return (DECIMAL_POW(x,2)-a) * (DECIMAL_POW(x,2)-b) - (c*x) + (c*s) - d;}
165
169decimal QuestCharPolyPrime(decimal x, decimal a, decimal b, decimal c) {return 4*DECIMAL_POW(x,3) - 2*(a+b)*x - c;}
170
177 decimal *eig) {
178 for (int i = 0; i < QUEST_MAX_ITERATIONS; i++) {
179 decimal height = QuestCharPoly(guess, a, b, c, d, s) / QuestCharPolyPrime(guess, a, b, c);
180 guess -= height;
181 if (DECIMAL_ABS(height) < EPSILON) {
182 *eig = guess;
183 return true;
184 }
185 }
186 return false;
187}
188
190 const Stars &stars,
191 const Catalog &catalog,
192 const StarIdentifiers &starIdentifiers) {
193
194 if (starIdentifiers.size() < 2) {
195 return Attitude();
196 }
197 assert(stars.size() >= 2);
198
199 // initial guess for eigenvalue (sum of the weights)
200 decimal guess = 0;
201
202 // attitude profile matrix
203 Mat3 B = {0, 0, 0, 0, 0, 0, 0, 0, 0};
204 for (const StarIdentifier &s : starIdentifiers) {
205 Star bStar = stars[s.starIndex];
206 Vec3 bStarSpatial = camera.CameraToSpatial(bStar.position).Normalize();
207
208 CatalogStar rStar = catalog[s.catalogIndex];
209 Vec3 rStarSpatial = {rStar.spatial.x, rStar.spatial.y, rStar.spatial.z};
210
211 //Weight = 1 (can be changed later, in which case we want to make a vector to hold all weights {ai})
212 // Calculate matrix B = sum({ai}{bi}{ri}T)
213 B = B + (rStarSpatial.OuterProduct(bStarSpatial) * s.weight);
214
215 // sum up the weights for initial guess of eigenvalue
216 guess += s.weight;
217 }
218
219 // S = B + Transpose(B)
220 Mat3 S = B + B.Transpose();
221 //sigma = B[0][0] + B[1][1] + B[2][2]
222 decimal sigma = B.Trace();
223 //Z = [[B[1][2] - B[2][1]], [B[2][0] - B[0][2]], [B[0][1] - B[1][0]]]
224 Vec3 Z = {
225 B.At(1,2) - B.At(2,1),
226 B.At(2,0) - B.At(0,2),
227 B.At(0,1) - B.At(1,0)
228 };
229
230 // calculate coefficients for characteristic polynomial
231 decimal delta = S.Det();
232 decimal kappa = (S.Inverse() * delta).Trace();
233 decimal a = DECIMAL_POW(sigma,2) - kappa;
234 decimal b = DECIMAL_POW(sigma,2) + (Z * Z);
235 decimal c = delta + (Z * S * Z);
236 decimal d = Z * (S * S) * Z;
237
238 // Newton-Raphson method for estimating the largest eigenvalue
239 decimal eig;
240 if (!QuestEigenvalueEstimator(guess, a, b, c, d, sigma, &eig)) {
241 // Didn't converge (can happen with 2 stars in float mode), so report no attitude
242 return Attitude();
243 }
244
245 // solve for the optimal quaternion: from https://ahrs.readthedocs.io/en/latest/filters/quest.html
246 decimal alpha = DECIMAL_POW(eig,2) - DECIMAL_POW(sigma, 2) + kappa;
247 decimal beta = eig - sigma;
248 decimal gamma = (eig + sigma) * alpha - delta;
249
250 Vec3 X = ((kIdentityMat3 * alpha) + (S * beta) + (S * S)) * Z;
251 decimal scalar = 1 / DECIMAL_SQRT(DECIMAL_POW(gamma,2) + X.MagnitudeSq());
252 X = X * scalar;
253 gamma *= scalar;
254
255 return Attitude(Quaternion(gamma, X.x, X.y, X.z));
256}
257
258}
#define EPSILON
#define QUEST_MAX_ITERATIONS
The attitude (orientation) of a spacecraft.
A full description of a camera. Enough information to reconstruct the camera matrix and then some.
Definition camera.hpp:9
Vec3 CameraToSpatial(const Vec2 &) const
Gives a point in 3d space that could correspond to the given vector, using the same coordinate system...
Definition camera.cpp:35
A star from the Bright Star Catalog.
Vec3 spatial
The point on the unit sphere where the star lies.
Attitude Go(const Camera &, const Stars &, const Catalog &, const StarIdentifiers &)
Actually run the star-id algorithm.
3x3 vector with decimaling point components
Mat3 Inverse() const
Inverse of a matrix.
decimal Det() const
Determinant of a matrix.
Mat3 Transpose() const
Transpose of a matrix.
decimal Trace() const
Trace of a matrix.
decimal At(int i, int j) const
Access the i,j-th element of the matrix.
A quaternion is a common way to represent a 3d rotation.
Attitude Go(const Camera &, const Stars &, const Catalog &, const StarIdentifiers &)
Actually run the star-id algorithm.
Records that a certain Star (detected in the image) corresponds to a certain CatalogStar.
int catalogIndex
An index into an array of CatalogStar objects.
int starIndex
An index into an array of Star objects.
A "centroid" detected in an image.
Vec2 position
The (x,y) pixel coordinates in the image (top left is 0,0).
Attitude Go(const Camera &, const Stars &, const Catalog &, const StarIdentifiers &)
Actually run the star-id algorithm.
Three dimensional vector with decimaling point components.
Vec3 Normalize() const
Create a vector pointing in the same direction with magnitude 1.
decimal MagnitudeSq() const
The square of the magnitude.
Mat3 OuterProduct(const Vec3 &) const
The outer product of two vectors.
Vec3 CrossProduct(const Vec3 &) const
Usual vector cross product.
double decimal
Definition decimal.hpp:8
#define DECIMAL_POW(base, power)
Definition decimal.hpp:36
#define DECIMAL_ABS(x)
Definition decimal.hpp:46
#define DECIMAL_SQRT(x)
Definition decimal.hpp:37
LOST starting point.
std::vector< StarIdentifier > StarIdentifiers
std::vector< Star > Stars
const Mat3 kIdentityMat3
3x3 identity matrix
bool QuestEigenvalueEstimator(decimal guess, decimal a, decimal b, decimal c, decimal d, decimal s, decimal *eig)
Approximates roots of a real function using the Newton-Raphson algorithm.
decimal QuestCharPoly(decimal x, decimal a, decimal b, decimal c, decimal d, decimal s)
Characteristic polynomial of the quest K-matrix.
decimal QuestCharPolyPrime(decimal x, decimal a, decimal b, decimal c)
Derivitive of the characteristic polynomial of the quest K-matrix.
std::vector< CatalogStar > Catalog