Merge pull request #126 from UWCubeSat/muki-decimal-#26

Decimal Type determined at Compile Time (#26)

.github/workflows/build-test-lint.yml

@@ -12,10 +12,14 @@ jobs:
 
     steps:
     - uses: actions/checkout@v3
-    - name: Build
+    - name: Build Double
       run: CXXFLAGS=-Werror make -j$(($(nproc)+1))
-    - name: Test
+    - name: Test Double
       run: CXXFLAGS=-Werror make test -j$(($(nproc)+1))
+    - name: Build Float
+      run: CXXFLAGS=-Werror make clean all LOST_FLOAT_MODE=1 -j$(($(nproc)+1))
+    - name: Test Float
+      run: CXXFLAGS=-Werror make test LOST_FLOAT_MODE=1 -j$(($(nproc)+1))
 
   lint:
     runs-on: ubuntu-latest

Makefile

@@ -1,4 +1,4 @@
-# Copyright (c) 2020 Mark Polyakov, Karen Haining (If you edit the file, add your name here!)
+# Copyright (c) 2020 Mark Polyakov, Karen Haining, Muki Kiboigo (If you edit the file, add your name here!)
 # 
 # Permission is hereby granted, free of charge, to any person obtaining a copy
 # of this software and associated documentation files (the "Software"), to deal
@@ -51,6 +51,12 @@ ifndef LOST_DISABLE_ASAN
 	LDFLAGS := $(LDFLAGS) -fsanitize=address
 endif
 
+# Use Double Mode by default.
+# If compiled with LOST_FLOAT_MODE=1, we will use floats.
+ifdef LOST_FLOAT_MODE
+	CXXFLAGS := $(CXXFLAGS) -Wdouble-promotion -Werror=double-promotion -D LOST_FLOAT_MODE
+endif
+
 all: $(BIN) $(BSC)
 
 release: CXXFLAGS := $(RELEASE_CXXFLAGS)

src/attitude-estimators.cpp

@@ -1,10 +1,13 @@
 #include "attitude-estimators.hpp"
+
 #include <eigen3/Eigen/Dense>
 #include <eigen3/Eigen/Eigenvalues>
 
+#include "decimal.hpp"
+
 namespace lost {
 
-#define EPSILON 0.0001       // threshold for 0 for Newton-Raphson method
+#define EPSILON DECIMAL(0.0001)       // threshold for 0 for Newton-Raphson method
 
 Attitude DavenportQAlgorithm::Go(const Camera &camera,
                                  const Stars &stars,
@@ -16,47 +19,82 @@ Attitude DavenportQAlgorithm::Go(const Camera &camera,
     assert(stars.size() >= 2);
 
     // attitude profile matrix
-    Eigen::Matrix3f B;
+    #ifdef LOST_FLOAT_MODE
+        Eigen::Matrix3f B;
+    #else
+        Eigen::Matrix3d B;
+    #endif
+
+
     B.setZero();
     for (const StarIdentifier &s : starIdentifiers) {
         Star bStar = stars[s.starIndex];
         Vec3 bStarSpatial = camera.CameraToSpatial(bStar.position);
-        Eigen::Vector3f bi;
+
+        #ifdef LOST_FLOAT_MODE
+            Eigen::Vector3f bi;
+            Eigen::Vector3f ri;
+        #else
+            Eigen::Vector3d bi;
+            Eigen::Vector3d ri;
+        #endif
+
         bi << bStarSpatial.x, bStarSpatial.y, bStarSpatial.z;
 
         CatalogStar rStar = catalog[s.catalogIndex];
-        Eigen::Vector3f ri;
         ri << rStar.spatial.x, rStar.spatial.y, rStar.spatial.z;
-
         //Weight = 1 (can be changed later, in which case we want to make a vector to hold all weights {ai})
         // Calculate matrix B = sum({ai}{bi}{ri}T)
         B += ri * bi.transpose() * s.weight;
     }
 
     // S = B + Transpose(B)
-    Eigen::Matrix3f S = B + B.transpose();
+    #ifdef LOST_FLOAT_MODE
+        Eigen::Matrix3f S = B + B.transpose();
+    #else
+        Eigen::Matrix3d S = B + B.transpose();
+    #endif
+
     //sigma = B[0][0] + B[1][1] + B[2][2]
-    float sigma = B.trace();
+    decimal sigma = B.trace();
+
     //Z = [[B[1][2] - B[2][1]], [B[2][0] - B[0][2]], [B[0][1] - B[1][0]]]
-    Eigen::Vector3f Z;
+    #ifdef LOST_FLOAT_MODE
+        Eigen::Vector3f Z;
+    #else
+        Eigen::Vector3d Z;
+    #endif
+
     Z << B(1,2) - B(2,1),
         B(2,0) - B(0,2),
         B(0,1) - B(1,0);
 
     // 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]]]
-    Eigen::Matrix4f K;
+    #ifdef LOST_FLOAT_MODE
+        Eigen::Matrix4f K;
+    #else
+        Eigen::Matrix4d K;
+    #endif
+
     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;
 
     //Find eigenvalues of K, store the largest one as lambda
     //find the maximum index
-    Eigen::EigenSolver<Eigen::Matrix4f> solver(K);
-    Eigen::Vector4cf values = solver.eigenvalues();
-    Eigen::Matrix4cf vectors = solver.eigenvectors();
+    #ifdef LOST_FLOAT_MODE
+        Eigen::EigenSolver<Eigen::Matrix4f> solver(K);
+        Eigen::Vector4cf values = solver.eigenvalues();
+        Eigen::Matrix4cf vectors = solver.eigenvectors();
+    #else
+        Eigen::EigenSolver<Eigen::Matrix4d> solver(K);
+        Eigen::Vector4cd values = solver.eigenvalues();
+        Eigen::Matrix4cd vectors = solver.eigenvectors();
+    #endif
+
     int maxIndex = 0;
-    float maxEigenvalue = values(0).real();
+    decimal maxEigenvalue = values(0).real();
     for (int i = 1; i < values.size(); i++) {
         if (values(i).real() > maxEigenvalue) {
             maxIndex = i;
@@ -65,7 +103,12 @@ Attitude DavenportQAlgorithm::Go(const Camera &camera,
     }
 
     //The return quaternion components = eigenvector assocaited with lambda
-    Eigen::Vector4cf maxEigenvector = vectors.col(maxIndex);
+    #ifdef LOST_FLOAT_MODE
+        Eigen::Vector4cf maxEigenvector = vectors.col(maxIndex);
+    #else
+        Eigen::Vector4cd maxEigenvector = vectors.col(maxIndex);
+    #endif
+
     // IMPORTANT: The matrix K is symmetric -- clearly first row and first column are equal.
     // 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
     // bottom right 3x3 block of K is symmetric too. Thus all its eigenvalues and eigenvectors
@@ -117,19 +160,19 @@ Attitude TriadAlgorithm::Go(const Camera &camera,
  * Characteristic polynomial of the quest K-matrix
  * @see equation 19b of https://arc.aiaa.org/doi/pdf/10.2514/1.62549
  */
-float QuestCharPoly(float x, float a, float b, float c, float d, float s) {return (pow(x,2)-a) * (pow(x,2)-b) - (c*x) + (c*s) - d;}
+decimal 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;}
 
 /**
  * Derivitive of the characteristic polynomial of the quest K-matrix
  */
-float QuestCharPolyPrime(float x, float a, float b, float c) {return 4*pow(x,3) - 2*(a+b)*x - c;}
+decimal QuestCharPolyPrime(decimal x, decimal a, decimal b, decimal c) {return 4*DECIMAL_POW(x,3) - 2*(a+b)*x - c;}
 
 /**
  * Approximates roots of a real function using the Newton-Raphson algorithm 
  * @see https://www.geeksforgeeks.org/program-for-newton-raphson-method/
  */
-float QuestEigenvalueEstimator(float guess, float a, float b, float c, float d, float s) {
-    float height;
+decimal QuestEigenvalueEstimator(decimal guess, decimal a, decimal b, decimal c, decimal d, decimal s) {
+    decimal height;
     do {
         height = QuestCharPoly(guess, a, b, c, d, s) / QuestCharPolyPrime(guess, a, b, c);
         guess -= height;
@@ -149,7 +192,7 @@ Attitude QuestAlgorithm::Go(const Camera &camera,
     assert(stars.size() >= 2);
 
     // initial guess for eigenvalue (sum of the weights)
-    float guess = 0;
+    decimal guess = 0;
 
     // attitude profile matrix
     Mat3 B = {0, 0, 0, 0, 0, 0, 0, 0, 0};
@@ -171,7 +214,7 @@ Attitude QuestAlgorithm::Go(const Camera &camera,
     // S = B + Transpose(B)
     Mat3 S = B + B.Transpose();
     //sigma = B[0][0] + B[1][1] + B[2][2]
-    float sigma = B.Trace();
+    decimal sigma = B.Trace();
     //Z = [[B[1][2] - B[2][1]], [B[2][0] - B[0][2]], [B[0][1] - B[1][0]]]
     Vec3 Z = {
         B.At(1,2) - B.At(2,1),
@@ -180,23 +223,23 @@ Attitude QuestAlgorithm::Go(const Camera &camera,
     };
 
     // calculate coefficients for characteristic polynomial
-    float delta = S.Det();
-    float kappa = (S.Inverse() * delta).Trace();
-    float a = pow(sigma,2) - kappa;
-    float b = pow(sigma,2) + (Z * Z);
-    float c = delta + (Z * S * Z);
-    float d = Z * (S * S) * Z;
+    decimal delta = S.Det();
+    decimal kappa = (S.Inverse() * delta).Trace();
+    decimal a = DECIMAL_POW(sigma,2) - kappa;
+    decimal b = DECIMAL_POW(sigma,2) + (Z * Z);
+    decimal c = delta + (Z * S * Z);
+    decimal d = Z * (S * S) * Z;
 
     // Newton-Raphson method for estimating the largest eigenvalue
-    float eig = QuestEigenvalueEstimator(guess, a, b, c, d, sigma);
+    decimal eig = QuestEigenvalueEstimator(guess, a, b, c, d, sigma);
 
     // solve for the optimal quaternion: from https://ahrs.readthedocs.io/en/latest/filters/quest.html
-    float alpha = pow(eig,2) - pow(sigma, 2) + kappa;
-    float beta = eig - sigma;
-    float gamma = (eig + sigma) * alpha - delta;
+    decimal alpha = DECIMAL_POW(eig,2) - DECIMAL_POW(sigma, 2) + kappa;
+    decimal beta = eig - sigma;
+    decimal gamma = (eig + sigma) * alpha - delta;
 
     Vec3 X = ((kIdentityMat3 * alpha) + (S * beta) + (S * S)) * Z;
-    float scalar = 1 / sqrt(pow(gamma,2) + X.MagnitudeSq());
+    decimal scalar = 1 / DECIMAL_SQRT(DECIMAL_POW(gamma,2) + X.MagnitudeSq());
     X = X * scalar;
     gamma *= scalar;