Triad-Based Attitude Filter Initialization Function

[kylekrol]

The UKF attitude filter requires an initial "guess" of our current attitude. On orbit, an easy way to accommodate this would be to initialize the filter once we have a GPS estimate, magnetometer readings, and sun vector reading.

The attitude UKF requires an initial "guess" of our attitude equation prior to simply calling it's update function on every control cycle. On orbit, one easy way to handle this is initializing the attitude filter based on a triad estimate once we have a valid GPS estimate, magnetometer reading, and sun vector reading.

Once #192 is merged, this code will need to be added to src/gnc_attitude_estimator.cpp per the inline documentation in include/gnc/attitude_estimator.hpp given below:

/** @fn attitude_estimator_reset
 *  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).
 *  @param[in]  r_ecef Position in ECEF (units of meters).
 *  @param[in]  b_body Magnetic field measured in the body frame (units of Tesla).
 *  @param[in]  s_body Sun vector measured in the body frame (unit vector).
 *
 *  Uses the current time, position, b field, and sun vector to calculate an
 *  attitude estimate to initialize the filter with. The other filter state
 *  variables are set to default values.
 *
 *  After calling, it's recommended to check the `state.is_valid` flag to ensure
 *  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);

A good reference to get started would be looking at the old estimator code here which performed triad given a sun vector, magnetic field reading, current position, and timestamp. Here is a link to an old version of the repo containing the previous estimator's code. And below is a snippet of the triad method being performed:

#include <gnc/constants.hpp>
#include <gnc/environment.hpp>
#include <gnc/utilities.hpp>

#include <lin/core.hpp>

// Determine the magnetic field in ECI (it's given in ECEF)
env::magnetic_field(t, lin::Vector3f(r_ecef), b_eci);  // b_eci = b_ecef
env::earth_attitude(t, q);                             // q = q_ecef_eci
utl::quat_conj(q);                                     // q = q_eci_ecef
utl::rotate_frame(q, b_eci);                           // b_eci = b_eci

// Ensure the measured and expected magnetic field are large enough
float thresh = constant::b_noise_floor * constant::b_noise_floor;
if ((lin::fro(b_eci) < thresh) || (lin::fro(b_body) < thresh))
  return;

// Determine the expected sun vector
env::sun_vector(t, s_eci);

// Ensure the sun vectors are unit vectors
GNC_ASSERT_NORMALIZED(s_body);
GNC_ASSERT_NORMALIZED(s_eci);

// Perform triad
// Remember the value of q_body_eci will be set to NaNs if triad fails
utl::triad(s_eci, (b_eci / lin::norm(b_eci)).eval(), s_body,
    (b_body / lin::norm(b_body)).eval(), q_body_eci);