Magnetometer & Sun Vector Attitude Filter Implementation (#194)

.gitignore

@@ -14,7 +14,7 @@ dist/
 downloads/
 eggs/
 .eggs/
-lib/
+#lib/
 lib64/
 parts/
 sdist/

include/gnc/attitude_estimator.hpp

@@ -24,8 +24,9 @@
 
 /** @file gnc/attitude_estimator.hpp
  *  @author Kyle Krol
- *  Defines the interface for the attitude estimator.
- * 
+ *
+ *  @brief Defines the interface for the attitude estimator.
+ *
  *  At a high level, the attitude estimator implemented here is an unscented
  *  Kalman filter estimating both attitude and gyro bias. It's based on "Unscented
  *  Filtering for Spacecraft Attitude Estimation" by John Crassidis and Landis
@@ -39,8 +40,7 @@
  *
  *  gnc::AttitudeEstimatorState state; 
  *
- *  void loop(double t, lin::Vector3d const &r_ecef,...,
- *      lin::Vector4f q_eci_body,...) {
+ *  void loop(double t, lin::Vector3d const &r_ecef, ..., lin::Vector4f q_eci_body, ...) {
  *    if (state.is_valid) {
  *      gnc::AttitudeEstimatorData data;
  *      data.t = t;
@@ -68,35 +68,73 @@
 #include <lin/core.hpp>
 
 namespace gnc {
+namespace constant {
+
+/** @brief Standard deviation of gyro noise.
+ *
+ *  This parameter tunes the expected amount of gyro noise within the attitude
+ *  estimator. It's defaulted to a value of `1.0e-6f` (units of radians per
+ *  second). */
+extern float ukf_sigma_v;
 
-/** @struct AttitudeEstimatorState
- *  Contains the internal state of an attitude estimator.
+/** @brief Standard deviation of gyro bias noise.
+ *
+ *  This parameter tunes the expected amount of gyro bias noise within the
+ *  attitude estimator. It's defaulted to a value of `2.75e-4f` (MKS units). */
+extern float ukf_sigma_u;
+
+/** @brief Standard deviation of magnetometer noise.
+ *
+ *  This parameter tunes the expected amount of magnetometer noise within the
+ *  attitude estimator. It's defaulted to a value of `5.0e-7f` (units of Tesla) */
+extern float ukf_sigma_b;
+
+/** @brief Standard deviation of sun vector noise (in terms of angle error).
+ *
+ *  This parameter tunes the expected amount of sun vector noise within the
+ *  attitude estimator. It's defaulted to a value of
+ *  `2.0f * constant::deg_to_rad_f` (units of radians). */
+extern float ukf_sigma_s;
+
+}  // namespace constant
+
+/** @brief Contains the internal state of an attitude estimator.
  *
  *  The internal state of the attitude estimator includes the previous estimates
- *  attitude quaternion, gyro bias estimate, state covariance matrix, and
- *  timestamp. After an `attitude_estimator_*` function call, the `is_valid`
- *  member variable can be read to check if all stored values are finite - i.e.
- *  not NaN or inifinity.
+ *  attitude quaternion, gyro bias estimate, state covariance matrix, timestamp,
+ *  and some other variables to act as a buffer for intermediate calculations.
+ *
+ *  After an `attitude_estimator_*` function call, the `is_valid` member variable
+ *  can be read to check if all stored values are finite - i.e. not NaN or
+ *  inifinity.
  *
  *  Prior to being used with the `attitude_estimator_update` function, the state
  *  variable must be initialized with a call to `attitude_estimator_reset`. See
  *  more documentation below. */
 struct AttitudeEstimatorState {
-  lin::Vectord<6> sigmas[13];
-  lin::Vectord<5> measures[13];
-  lin::Vector4f q_body_eci;
-  lin::Vector3f gyro_bias;
-  lin::Matrixf<6, 6> P;
+  /** Variables acting as a calculation buffer
+   *  @{ */
+  lin::Vectord<6>    x_bar, sigmas[13];
+  lin::Vectord<5>    z_bar, measures[13];
+  lin::Matrixd<6, 6> P_bar;
+  lin::Matrixd<5, 5> P_vv;
+  lin::Matrixd<6, 5> P_xy;
+  /** @} */
+  /** Persistant, state varibles
+   *  @{ */
+  lin::Vectord<4> q;
+  lin::Vectord<6> x;
+  lin::Matrixd<6, 6> P;
   double t;
+  /** @} */
   /** Default everything to NaN and sets `is_valid` to `false`. */
   AttitudeEstimatorState();
   /** Signals whether the struct specifies a valid filter state. If invalid, all
    *  data members will have been set to NaN. */
   bool is_valid;
 };
 
-/** @struct AttitudeEstimatorData
- *  Stores inputs to the `attitude_estimator_update` function.
+/** @brief Stores inputs to the `attitude_estimator_update` function.
  *
  *  These inputs include the current time since the PAN epoch (in seconds),
  *  position in ECEF (in meters), magnetic field reading in the body frame (in
@@ -117,8 +155,7 @@ struct AttitudeEstimatorData {
   AttitudeEstimatorData();
 };
 
-/** @struct AttitudeEstimate
- *  Stores outputs of the `attitude_estimator_update` function.
+/** @brief Stores outputs of the `attitude_estimator_update` function.
  *
  *  These outputs are the current attitude in quaternion representation
  *  (quaternion rotating from ECI to the the body frame), gyro bias estimate in
@@ -144,8 +181,20 @@ struct AttitudeEstimate {
   bool is_valid;
 };
 
-/** @fn attitude_estimator_reset
- *  Initializes the attitude filter state using the triad method.
+/** @brief Initializes the attitude filter state.
+ *
+ *  @param[out] state      Attitude estimator state to be reset/initialized.
+ *  @param[in]  t          Time since the PAN epoch (units of seconds).
+ *  @param[in]  q_body_eci Quaternion rotating from ECI to the body frame.
+ *
+ *  Initializes the current time and attitude estimate to the passed arguments.
+ *  The gyro bias and covariance is set to default values.
+ *
+ *  The state will be valid as long as the passed arguments were finite. */
+void attitude_estimator_reset(AttitudeEstimatorState &state,
+    double t, lin::Vector4f const &q_body_eci);
+
+/** @brief Initializes the attitude filter state using the triad method.
  *
  *  @param[out] state  Attitude estimator state to be reset/initialized.
  *  @param[in]  t      Time since the PAN epoch (units of seconds).
@@ -161,38 +210,24 @@ struct AttitudeEstimate {
  *  the estimator state was succesfully reset. The reset call will fail is
  *  parameters given failed the triad algorithm - i.e the sun and magnetic field
  *  vector were nearly parallel.
- * 
+ *
  *  See the `gnc::utl::triad` documentation for more information. */
 void attitude_estimator_reset(AttitudeEstimatorState &state,
     double t, lin::Vector3d const &r_ecef, lin::Vector3f const &b_body,
     lin::Vector3f const &s_body);
 
-/** @fn attitude_estimator_reset
- *  Initializes the attitude filter state.
- *
- *  @param[out] state      Attitude estimator state to be reset/initialized.
- *  @param[in]  t          Time since the PAN epoch (units of seconds).
- *  @param[in]  q_eci_body Quaternion rotating from ECI to the body frame.
+/** @brief Updates the attitude estimate given a set of sensor readings.
  *
- *  Initializes the current time and attitude estimate to the passed arguments.
- *  The gyro bias and covariance is set to default values.
- *
- *  The state will be valid as long as the passed arguments were finite. */
-void attitude_estimator_reset(AttitudeEstimatorState &state,
-    double t, lin::Vector4f const &q_eci_body);
-
-/** @fn attitude_estimator_update
- *  Updates the attitude estimate given a set of sensor readings.
- * 
  *  @param[inout] state    Previous filter state.
  *  @param[in]    data     Sensor readings.
  *  @param[out]   estimate Updated attitude estimate.
  * 
  *  Calculates an updated attitude estimate using one of two unscented Kalman
  *  filter implementations. The first runs if all required elements of the `data`
  *  struct as well an a sun vector readings are specified; it's the superior
- *  filter. The latter runs when no sun vector is specified.
- * 
+ *  filter. The latter runs when no sun vector is specified. The estimator state
+ *  and estimate will default to invalid if some expected input isn't provided.
+ *
  *  After calling, it's recommended to check the `state.is_valid` or
  *  `estiamte.is_valid` field to ensure the update step completed succesfully.
  *  Further checks on the updated covariance matrix are also suggested to check

include/gnc/config.hpp

@@ -6,6 +6,8 @@
 #ifndef GNC_CONFIG_HPP_
 #define GNC_CONFIG_HPP_
 
+#include <lin/core.hpp>
+
 #include <cmath>
 #include <limits>
 #include <type_traits>

include/gnc/environment.hpp

@@ -11,34 +11,36 @@ namespace env {
 
 /** @fn earth_attitude
  *  @param[in]  t Time in seconds since the PAN epoch.
- *  @param[out] q Quaternion to transform vectors from ECI to ECEF. */
+ *  @param[out] q Quaternion to transform vectors from ECI to ECEF.
+ *  @{ */
 void earth_attitude(double t, lin::Vector4d &q);
-
-/** @fn earth_attitude
- *  @param[in]  t Time in seconds since the PAN epoch.
- *  @param[out] q Quaternion to transform vectors from ECI to ECEF. */
 void earth_attitude(double t, lin::Vector4f &q);
+/** @} */
 
 /** @fn earth_angular_rate
  *  @param[in]  t Time in seconds since the PAN epoch.
- *  @param[out] w Angular rate to transform vectors from ECI to ECEF. */
+ *  @param[out] w Angular rate to transform vectors from ECI to ECEF.
+ *  @{ */
 void earth_angular_rate(double t, lin::Vector3d &w);
-
-/** @fn earth_angular_rate
- *  @param[in]  t Time in seconds since the PAN epoch.
- *  @param[out] w Angular rate to transform vectors from ECI to ECEF. */
 void earth_angular_rate(double t, lin::Vector3f &w);
+/** @} */
 
 /** @fn sun_vector
  *  @param[in]  t Time in seconds since the PAN epoch.
- *  @param[out] s Normalized vector from Earth the Sun in the ECI frame. */
+ *  @param[out] s Normalized vector from Earth the Sun in the ECI frame.
+ *  @{ */
 void sun_vector(double t, lin::Vector3f &s);
+void sun_vector(double t, lin::Vector3d &s);
+/** @} */
 
 /** @fn magnetic_field
  *  @param[in]  t Time in seconds since the PAN epoch.
  *  @param[in]  r Position in the ECEF frame with units of meters.
- *  @param[out] b Magnetic field in the ECEF frame with units of Tesla. */
-void magnetic_field(double t, lin::Vector3f const &r, lin::Vector3f &b);
+ *  @param[out] b Magnetic field in the ECEF frame with units of Tesla.
+ *  @{ */
+void magnetic_field(double t, lin::Vector3d const &r, lin::Vector3f &b);
+void magnetic_field(double t, lin::Vector3d const &r, lin::Vector3d &b);
+/** @} */
 
 }  // namespace env
 }  // namespace gnc

include/gnc/inl/utilities.inl

@@ -30,6 +30,19 @@ constexpr void quat_cross_mult(lin::Vector<T, 4> const &q1, lin::Vector<T, 4> co
   res(3) = q1(3) * q2(3) - lin::dot(qv1, qv2);
 }
 
+template <typename T>
+constexpr void quat_to_qrp(lin::Vector<T, 4> const &q, T a, T f, lin::Vector<T, 3> &p) {
+  p = lin::ref<3, 1>(q, 0, 0) * (f / (a + q(3)));
+}
+
+template <typename T>
+constexpr void grp_to_quat(lin::Vector<T, 3> const &p, T a, T f, lin::Vector<T, 4> &q) {
+  T fro_p = lin::fro(p);
+  T sqr_f = f * f;
+  q(3) = (f * std::sqrt(sqr_f + (1 - a * a) * fro_p) - a * fro_p) / (sqr_f + fro_p);
+  lin::ref<3, 1>(q, 0, 0) = p * ((a + q(3)) / f);
+}
+
 template <typename T>
 constexpr void rotate_frame(lin::Vector<T, 4> const &q, lin::Vector<T, 3> const &v,
     lin::Vector<T, 3> &res) {

include/gnc/utilities.hpp

@@ -47,6 +47,27 @@ template <typename T>
 constexpr void quat_cross_mult(lin::Vector<T, 4> const &q1,
         lin::Vector<T, 4> const &q2, lin::Vector<T, 4> &res);
 
+/** @fn quat_to_grp
+ *  @param[in]  q Input quaternion.
+ *  @param[in]  a 
+ *  @param[in]  f
+ *  @param[out] p Generalized Rodrigues parameter representation.
+ *  Converts a quaternion attitude representation to generalized Rodrigues
+ *  parameters. There is no explicit handling of NaNs built into this function;
+ *  however, a finite input will always yield a finite result.
+ *  Source: "Unscented Filtering for Spacecraft Attitude Estimation"
+ *          by Markley and Crasidis */
+template <typename T>
+constexpr void quat_to_qrp(lin::Vector<T, 4> const &q, T a, T f, lin::Vector<T, 3> &p);
+
+/** @fn grp_to_quat
+ *  @param[in]  p Input Rodrigues parameters.
+ *  @param[in]  a
+ *  @param[in]  f
+ *  @param[out] q Quaternion representation. */
+template <typename T>
+constexpr void grp_to_quat(lin::Vector<T, 3> const &p, T a, T f, lin::Vector<T, 4> &q);
+
 /** @fn rotate_frame
  *  @param[in]  q   Quaternion specifying the frame rotation.
  *  @param[in]  v   Vector to be transformed.

platformio.ini

@@ -15,6 +15,7 @@ extends = all
 build_flags =
   ${all.build_flags}
   -DTEST_DESKTOP
+  -DLIN_DESKTOP
   -DDESKTOP
   -O2
   -std=c++14