diff --git a/.gitignore b/.gitignore index 809f8a2f..9e0fcb1e 100644 --- a/.gitignore +++ b/.gitignore @@ -14,7 +14,7 @@ dist/ downloads/ eggs/ .eggs/ -lib/ +#lib/ lib64/ parts/ sdist/ diff --git a/include/gnc/attitude_estimator.hpp b/include/gnc/attitude_estimator.hpp index c01b1494..c603a7aa 100644 --- a/include/gnc/attitude_estimator.hpp +++ b/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,26 +68,65 @@ #include 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 @@ -95,8 +134,7 @@ struct AttitudeEstimatorState { 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,29 +210,14 @@ 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. @@ -191,8 +225,9 @@ void attitude_estimator_reset(AttitudeEstimatorState &state, * 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 diff --git a/include/gnc/config.hpp b/include/gnc/config.hpp index 4474fa18..239dcf2d 100644 --- a/include/gnc/config.hpp +++ b/include/gnc/config.hpp @@ -6,6 +6,8 @@ #ifndef GNC_CONFIG_HPP_ #define GNC_CONFIG_HPP_ +#include + #include #include #include diff --git a/include/gnc/environment.hpp b/include/gnc/environment.hpp index 384ddd96..e603c0ce 100644 --- a/include/gnc/environment.hpp +++ b/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 diff --git a/include/gnc/inl/utilities.inl b/include/gnc/inl/utilities.inl index 26f57878..18018300 100644 --- a/include/gnc/inl/utilities.inl +++ b/include/gnc/inl/utilities.inl @@ -30,6 +30,19 @@ constexpr void quat_cross_mult(lin::Vector const &q1, lin::Vector co res(3) = q1(3) * q2(3) - lin::dot(qv1, qv2); } +template +constexpr void quat_to_qrp(lin::Vector const &q, T a, T f, lin::Vector &p) { + p = lin::ref<3, 1>(q, 0, 0) * (f / (a + q(3))); +} + +template +constexpr void grp_to_quat(lin::Vector const &p, T a, T f, lin::Vector &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 constexpr void rotate_frame(lin::Vector const &q, lin::Vector const &v, lin::Vector &res) { diff --git a/include/gnc/utilities.hpp b/include/gnc/utilities.hpp index 1bac1ac9..e08251fd 100644 --- a/include/gnc/utilities.hpp +++ b/include/gnc/utilities.hpp @@ -47,6 +47,27 @@ template constexpr void quat_cross_mult(lin::Vector const &q1, lin::Vector const &q2, lin::Vector &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 +constexpr void quat_to_qrp(lin::Vector const &q, T a, T f, lin::Vector &p); + +/** @fn grp_to_quat + * @param[in] p Input Rodrigues parameters. + * @param[in] a + * @param[in] f + * @param[out] q Quaternion representation. */ +template +constexpr void grp_to_quat(lin::Vector const &p, T a, T f, lin::Vector &q); + /** @fn rotate_frame * @param[in] q Quaternion specifying the frame rotation. * @param[in] v Vector to be transformed. diff --git a/platformio.ini b/platformio.ini index c3cc98e5..18bcb512 100644 --- a/platformio.ini +++ b/platformio.ini @@ -15,6 +15,7 @@ extends = all build_flags = ${all.build_flags} -DTEST_DESKTOP + -DLIN_DESKTOP -DDESKTOP -O2 -std=c++14 diff --git a/src/gnc_attitude_estimator.cpp b/src/gnc_attitude_estimator.cpp index befb98cd..0c15e561 100644 --- a/src/gnc_attitude_estimator.cpp +++ b/src/gnc_attitude_estimator.cpp @@ -28,16 +28,348 @@ #include #include #include +#include +#include #include +#include +#include #include +#include +#include +#include +#include +#include namespace gnc { +namespace constant { + +GNC_TRACKED_CONSTANT(float, ukf_sigma_v, 1.0e-6f); +GNC_TRACKED_CONSTANT(float, ukf_sigma_u, 2.75e-4f); +GNC_TRACKED_CONSTANT(float, ukf_sigma_b, 5.0e-7f); +GNC_TRACKED_CONSTANT(float, ukf_sigma_s, 2.0f * constant::deg_to_rad_f); + +} // namespace constant + +/** Specifies the floating point type used for most internal UKF calculations. */ +typedef double ukf_float; + +/** Useful type definitions for the filter implementation. + * @{ */ +typedef lin::Vector UkfVector2; +typedef lin::Vector UkfVector3; +typedef lin::Vector UkfVector4; +typedef lin::Vector UkfVector5; +typedef lin::Vector UkfVector6; +typedef lin::Matrix UkfMatrix3x3; +typedef lin::Matrix UkfMatrix4x4; +typedef lin::Matrix UkfMatrix5x5; +typedef lin::Matrix UkfMatrix5x6; +typedef lin::Matrix UkfMatrix6x5; +typedef lin::Matrix UkfMatrix6x6; +/** @} */ + +/** @fn ukf_propegate + * Propegates forward the attitude of a rotating rigid body. + * + * @param[in] dt Timestep (seconds). + * @param[in] w Angular rate (radians per second). + * @param[in] q_old Initial attitude. + * @param[out] q_new Final attitude. + * + * This function assuming the body is rotating at a constant angular rate and + * therefore may only be accurate for small timesteps. */ +static void ukf_propegate(ukf_float dt, UkfVector3 const &w, + UkfVector4 const &q_old, UkfVector4 &q_new) { + GNC_ASSERT_NORMALIZED(q_old); + + // Calculate transition matrix + UkfMatrix4x4 O; + { + ukf_float norm_w = lin::norm(w); + UkfVector3 psi = lin::sin(0.5f * dt * norm_w) * w / norm_w; + UkfMatrix3x3 psi_x = { + ukf_float(0.0), -psi(2), psi(1), + psi(2), ukf_float(0.0), -psi(0), + -psi(1), psi(0), ukf_float(0.0) + }; + O = lin::cos(0.5f * norm_w * dt) * lin::identity(); + lin::ref<3, 3>(O, 0, 0) = lin::ref<3, 3>(O, 0, 0) - psi_x; + lin::ref<3, 1>(O, 0, 3) = psi; + lin::ref<1, 3>(O, 3, 0) = -lin::transpose(psi); + } + + // Propegate our quaternion forward and normalize to be safe + q_new = O * q_old; + q_new = q_new / lin::norm(q_new); +} + +/** @fn ukf + * Performs a single attitude estimator update step. + * + * @param[inout] state Attitude estimator state. + * @param[in] data Input sensor measurements. + * @param[in] ukf_kalman_update Kalman gain update step function. + * + * This functions takes `state.q`, `state.x`, `state.t`, and `data.*` (with the + * potential exception of `data.s_body`) as inputs. It's assumed that the values + * in both structs are finite. + * + * This function will determine sigma points, propegate sigma points, simulate + * expected measurements, and populate the `x_bar`, `z_bar`, `P_bar`, `P_vv`, + * and `P_xy` fields of the estimator state. + * + * The `ukf_kalman_update` function will then be called and is responsible for + * calculating the Kalman gain and updating the estimator state vector + * (`state.x`) and covariance (`state.P`). This was done so the magnetometer only + * and magnetomter plus sun vector filters could share a large amount of code. + * + * Once the function returns, `ukf` will update `state.t`, zero the first three + * components of `state.x` (zeroth sigma point has "no" attitude error), and + * update `state.q`. */ +static void ukf(AttitudeEstimatorState &state, AttitudeEstimatorData const &data, + void (*ukf_kalman_update)(AttitudeEstimatorState &state, AttitudeEstimatorData const &data)) { + /** Tuning parameter for the shape of the sigma point distribution. */ + constexpr static ukf_float lambda = 1.0; + /** GRP conversion parameters. @{ */ + constexpr static ukf_float a = 1.0; + constexpr static ukf_float f = 2.0 * (a + 1.0); + /** @} + * Length of a state vector. */ + constexpr static ukf_float N = 6.0; + + // Timestep + ukf_float dt; + { + dt = data.t - state.t; + /** Smallest possible timestep we'll allow this filter to take. */ + GNC_TRACKED_CONSTANT(constexpr static ukf_float, dt_thresh, 1.0e-3); + /* This check is to ensure we don't initialize and call update on the filter + * in the same timestep. */ + if (dt < dt_thresh) return; + } + + // Process noise covariance + UkfMatrix6x6 Q = lin::zeros(); + { + /** Tuning factor scaling the process noise covariance matrix. */ + GNC_TRACKED_CONSTANT(constexpr static ukf_float, Q_factor, 1.0e3); + + ukf_float factor = Q_factor * dt / 2.0f; + ukf_float var_u = constant::ukf_sigma_u * constant::ukf_sigma_u; + ukf_float var_v = constant::ukf_sigma_v * constant::ukf_sigma_v; + Q(0, 0) = factor * (var_v - var_u * dt * dt / 6.0f); + Q(1, 1) = Q(0, 0); + Q(2, 2) = Q(0, 0); + Q(3, 3) = factor * var_u; + Q(4, 4) = Q(3, 3); + Q(5, 5) = Q(3, 3); + } + + // Generate sigma points + { + UkfMatrix6x6 L = state.P + Q; + lin::chol(L); + + state.sigmas[0] = state.x; + L = lin::sqrt(N + lambda) * L; + for (lin::size_t i = 0; i < L.cols(); i++) state.sigmas[i + 1] = state.x + lin::ref_col(L, i); + for (lin::size_t i = 0; i < L.cols(); i++) state.sigmas[i + L.cols() + 1] = state.x - lin::ref_col(L, i); + } + + // Propegate the center sigma points attitude + UkfVector4 q_new; + { + UkfVector3 w = data.w_body - lin::ref<3, 1>(state.x, 3, 0); + ukf_propegate(dt, w, state.q, q_new); + } + + // Propegate sigma points forward and calculate expected measurements + { + // Generate expected sun and magnetic field vectors + UkfVector3 s_exp, b_exp; + { + env::sun_vector(data.t, s_exp); // s_exp in ECI + + UkfVector4 q_eci_ecef; + env::earth_attitude(data.t, q_eci_ecef); // q_eci_ecef = q_ecef_eci + utl::quat_conj(q_eci_ecef); // q_eci_ecef = q_eci_ecef + + env::magnetic_field(data.t, data.r_ecef, b_exp); // b_exp in ECEF + utl::rotate_frame(q_eci_ecef, b_exp); // b_exp in ECI + } + + // Calculate the expected measurements for the zeroth sigma point + { + UkfVector3 s, b; + utl::rotate_frame(q_new, s_exp, s); // s in the body frame + utl::rotate_frame(q_new, b_exp, b); // b in the body frame + + state.measures[0] = { + lin::atan(s(1) / s(0)), + lin::acos(s(2)), + b(0), + b(1), + b(2) + }; + } + + // Conjugate of the q_new + UkfVector4 conj_q_new; + utl::quat_conj(q_new, conj_q_new); + + // Propegate and generate expected measurements for the other sigma points + for (lin::size_t i = 1; i < 13; i++) { + // Calculate this sigma's propegated attitude + UkfVector4 _q_new; + { + UkfVector4 q, _q_old; + utl::grp_to_quat(lin::ref<3, 1>(state.sigmas[i], 0, 0).eval(), a, f, q); + utl::quat_cross_mult(q, UkfVector4(state.q), _q_old); + + UkfVector3 w = data.w_body - lin::ref<3, 1>(state.sigmas[i], 3, 0); + ukf_propegate(dt, w, _q_old, _q_new); + } + + // Determine expected measurements + { + UkfVector3 s, b; + utl::rotate_frame(_q_new, s_exp, s); // s in the body frame + utl::rotate_frame(_q_new, b_exp, b); // b_exp in the body frame + + state.measures[i] = { + lin::atan(s(1) / s(0)), + lin::acos(s(2)), + b(0), + b(1), + b(2) + }; + } + + // Determine the propegated sigmas + { + lin::Vector4d q; + utl::quat_cross_mult(_q_new, conj_q_new, q); // q = "residual" propegated rotation + + lin::Vector3d p; + utl::quat_to_qrp(q, a, f, p); + lin::ref<3, 1>(state.sigmas[i], 0, 0) = p; + } + } + } + + // Calculate mean expected state, mean expected measurements, and associated + // covariances + { + UkfVector6 &x_bar = state.x_bar; + UkfVector5 &z_bar = state.z_bar; + UkfMatrix6x6 &P_bar = state.P_bar; + UkfMatrix5x5 &P_vv = state.P_vv; + UkfMatrix6x5 &P_xy = state.P_xy; + + // Calculate sigma point and covariance weights + ukf_float weight_c = lambda / (N + lambda); + ukf_float weight_o = 1.0f / (2.0f * (N + lambda)); + + // Calculate x_bar and z_bar + x_bar = weight_c * state.sigmas[0]; + z_bar = weight_c * state.measures[0]; + for (lin::size_t i = 1; i < 13; i++) { + x_bar = x_bar + weight_o * state.sigmas[i]; + z_bar = z_bar + weight_o * state.measures[i]; + } + + // Plus the associated covariances + UkfVector6 dx1 = state.sigmas[0] - x_bar; + UkfVector5 dz1 = state.measures[0] - z_bar; + P_bar = P_bar + weight_c * dx1 * lin::transpose(dx1); + P_vv = P_vv + weight_c * dz1 * lin::transpose(dz1); + P_xy = P_xy + weight_c * dx1 * lin::transpose(dz1); + for (lin::size_t i = 1; i < 13; i++) { + dx1 = state.sigmas[i] - x_bar; + dz1 = state.measures[i] - z_bar; + P_bar = P_bar + weight_o * (dx1 * lin::transpose(dx1)); + P_vv = P_vv + weight_o * (dz1 * lin::transpose(dz1)); + P_xy = P_xy + weight_o * (dx1 * lin::transpose(dz1)); + } + + // Sensor noise covariance + UkfMatrix5x5 R = lin::zeros(); + { + R(0, 0) = constant::ukf_sigma_s * constant::ukf_sigma_s; + R(1, 1) = R(0, 0); + R(2, 2) = constant::ukf_sigma_b * constant::ukf_sigma_b; + R(3, 3) = R(2, 2); + R(4, 4) = R(2, 2); + } + + P_bar = P_bar + Q; // Add process noise to predicted covariance + P_vv = P_vv + R; // Add sensor noise to innovation covariance + } + + // Kalman gain step + ukf_kalman_update(state, data); + + // Process x (which is now x_new) and P (which is now P_new) + { + // Update time + state.t = data.t; + + // Perturb q_new according to the new state + lin::Vector4d q; + utl::grp_to_quat(lin::ref<3, 1>(state.x, 0, 0).eval(), a, f, q); + utl::quat_cross_mult(q, q_new, state.q); + + // Reset the attitude portion of the state to zeros + lin::ref<3, 1>(state.x, 0, 0) = lin::zeros(); + } +} + +/** @fn ukf_m + * Update attitude estimator state given a magnetometer reading. + * + * @param[inout] state Attitude filter state. + * @param[in] data Input sensor data. */ +static void ukf_m(AttitudeEstimatorState &state, AttitudeEstimatorData const &data) { + // TODO : Implement this + state = AttitudeEstimatorState(); +} + +/** @fn ukf_ms + * Update attitude estimator state given magnetometer and sun vector readings. + * + * @param[inout] state Attitude filter state. + * @param[in] data Input sensor data. */ +static void ukf_ms(AttitudeEstimatorState &state, AttitudeEstimatorData const &data) { + ukf(state, data, [](AttitudeEstimatorState &state, AttitudeEstimatorData const &data) -> void { + // Calculate Kalman gain + UkfMatrix6x5 K; + { + UkfMatrix5x5 Q, R; + lin::qr(state.P_vv, Q, R); + lin::backward_sub(R, Q, lin::transpose(Q).eval()); // Q = inv(P_yy) + K = state.P_xy * Q; + } + + // Calculate this steps measurement + UkfVector5 z_new { + lin::atan(data.s_body(1) / data.s_body(0)), + lin::acos(data.s_body(2)), + data.b_body(0), + data.b_body(1), + data.b_body(2) + }; + + // Update the state vector and covariance + state.x = state.x_bar + K * (z_new - state.z_bar).eval(); + state.P = state.P_bar - K * (state.P_vv * lin::transpose(K)).eval(); + }); +} AttitudeEstimatorState::AttitudeEstimatorState() -: q_body_eci(lin::nans()), - gyro_bias(lin::nans()), - P(lin::nans>()), +: q(lin::nans()), + x(lin::nans()), + P(lin::nans()), t(constant::nan), is_valid(false) { } @@ -55,26 +387,92 @@ AttitudeEstimate::AttitudeEstimate() is_valid(false) { } void attitude_estimator_reset(AttitudeEstimatorState &state, - double t, lin::Vector3d const &r_ecef, lin::Vector3f const &b_body, - lin::Vector3f const &s_body) { - GNC_ASSERT_NORMALIZED(s_body); + double t, lin::Vector4f const &q_body_eci) { + GNC_ASSERT_NORMALIZED(q_body_eci); - // TODO : Implement this - state = AttitudeEstimatorState(); + /* Default, initial attitude covariance (units of radiancs squared). */ + GNC_TRACKED_CONSTANT(constexpr static float, var_q, 0.0305); + /* Default, initial gyro bias covariance (units of radians per second all + * squared). */ + GNC_TRACKED_CONSTANT(constexpr static float, var_g, 0.0049); + /* Default, initial state. */ + GNC_TRACKED_CONSTANT(constexpr static UkfVector6, init_state, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + + // Set the proper fields in state + state.t = t; + state.q = q_body_eci; + state.x = init_state; + state.P = lin::zeros(); + state.P(0, 0) = var_q; + state.P(1, 1) = state.P(0, 0); + state.P(2, 2) = state.P(0, 0); + state.P(3, 3) = var_g; + state.P(4, 4) = state.P(3, 3); + state.P(5, 5) = state.P(4, 4); + state.is_valid = true; + + // If invalid, set everything back to NaNs + if (!lin::isfinite(state.t) || + !lin::all(lin::isfinite(state.q)) || + !lin::all(lin::isfinite(state.x)) || + !lin::all(lin::isfinite(state.P))) + state = AttitudeEstimatorState(); } void attitude_estimator_reset(AttitudeEstimatorState &state, - double t, lin::Vector4f const &q_eci_body) { - GNC_ASSERT_NORMALIZED(q_eci_body); + double t, lin::Vector3d const &r_ecef, lin::Vector3f const &b_body, + lin::Vector3f const &s_body) { + GNC_ASSERT_NORMALIZED(s_body); // TODO : Implement this - state = AttitudeEstimatorState(); + // https://github.com/pathfinder-for-autonomous-navigation/psim/issues/187 + lin::Vector4f q_body_eci = lin::nans(); + attitude_estimator_reset(state, t, q_body_eci); } void attitude_estimator_update(AttitudeEstimatorState &state, AttitudeEstimatorData const &data, AttitudeEstimate &estimate) { - // TODO : Implement this - state = AttitudeEstimatorState(); - estimate = AttitudeEstimate(); + // Ensure we have a valid state and all the required inputs + if (!state.is_valid || + !lin::all(lin::isfinite(data.r_ecef)) || + !lin::all(lin::isfinite(data.b_body)) || + !lin::all(lin::isfinite(data.w_body)) || + !lin::isfinite(data.t)) { + // Set the state and estimate to be invalid + state = AttitudeEstimatorState(); + estimate = AttitudeEstimate(); + return; + } + + GNC_ASSERT(lin::all(lin::isfinite(state.q))); + GNC_ASSERT(lin::all(lin::isfinite(state.x))); + GNC_ASSERT(lin::all(lin::isfinite(state.P))); + GNC_ASSERT_NORMALIZED(state.q); + + // Run the magnetomter and sun vector implementation + if (lin::all(lin::isfinite(data.s_body))) { + GNC_ASSERT_NORMALIZED(data.s_body); + ukf_ms(state, data); + } + // Run the magnetometer only implementation + else { + ukf_m(state, data); + } + + // Update resulted in an invalid state + if (!lin::all(lin::isfinite(state.q)) || + !lin::all(lin::isfinite(state.x)) || + !lin::all(lin::isfinite(state.P)) || + !lin::isfinite(state.t)) { + state = AttitudeEstimatorState(); + estimate = AttitudeEstimate(); + } + // Update gave a valid output + else { + estimate.q_body_eci = state.q; + estimate.gyro_bias = lin::ref<3, 1>(state.x, 3, 0); + estimate.P = state.P; + estimate.is_valid = true; + } } } // namespace gnc diff --git a/src/gnc_environment.cpp b/src/gnc_environment.cpp index 7367dfc0..2cfbc4a7 100644 --- a/src/gnc_environment.cpp +++ b/src/gnc_environment.cpp @@ -68,7 +68,13 @@ void sun_vector(double t, lin::Vector3f &s) { utl::rotate_frame(q_eci_perifocal_f, s); } -void magnetic_field(double t, lin::Vector3f const &r, lin::Vector3f &b) { +void sun_vector(double t, lin::Vector3d &s) { + lin::Vector3f _s; + sun_vector(t, _s); + s = _s; +} + +void magnetic_field(double t, lin::Vector3d const &r, lin::Vector3f &b) { geomag::Vector in, out; in.x = r(0); in.y = r(1); @@ -76,5 +82,11 @@ void magnetic_field(double t, lin::Vector3f const &r, lin::Vector3f &b) { out = geomag::GeoMag(constant::init_dec_year + t / (365.0 * 24.0 * 60.0 * 60.0),in, geomag::WMM2020); b = { out.x, out.y, out.z }; } + +void magnetic_field(double t, lin::Vector3d const &r, lin::Vector3d &b) { + lin::Vector3f _b; + magnetic_field(t, r, _b); + b = _b; +} } // namespace env } // namespace gnc diff --git a/test/test_attitude_estimator/test_attitude_estimator.cpp b/test/test_attitude_estimator/test_attitude_estimator.cpp index fd2fd663..89cd215d 100644 --- a/test/test_attitude_estimator/test_attitude_estimator.cpp +++ b/test/test_attitude_estimator/test_attitude_estimator.cpp @@ -1,25 +1,33 @@ #include +#include #include +#include #include #include #include #undef isnan #undef isinf +#undef isfinite -static void test_attitude_estimator_state_constructor() { +#define TEST_ASSERT_LIN_NEAR_ABS(tol, e, a) \ + TEST_ASSERT_TRUE(lin::all(tol > lin::abs(e - a))); static_assert(true, "") +#define TEST_ASSERT_LIN_FRO_NEAR_REL(tol, e, a) \ + TEST_ASSERT_TRUE(tol > lin::abs(lin::fro(e) - lin::fro(a)) / lin::fro(e)); static_assert(true, "") + +static void test_state_constructor() { gnc::AttitudeEstimatorState state; - TEST_ASSERT_TRUE(lin::all(lin::isnan(state.q_body_eci))); - TEST_ASSERT_TRUE(lin::all(lin::isnan(state.gyro_bias))); + TEST_ASSERT_TRUE(lin::all(lin::isnan(state.x))); + TEST_ASSERT_TRUE(lin::all(lin::isnan(state.x))); TEST_ASSERT_TRUE(lin::all(lin::isnan(state.P))); TEST_ASSERT_TRUE(lin::isnan(state.t)); TEST_ASSERT_FALSE(state.is_valid); } -static void test_attitude_estimator_data_constructor() { +static void test_data_constructor() { gnc::AttitudeEstimatorData data; TEST_ASSERT_TRUE(lin::all(lin::isnan(data.r_ecef))); @@ -29,7 +37,7 @@ static void test_attitude_estimator_data_constructor() { TEST_ASSERT_TRUE(lin::isnan(data.t)); } -static void test_attitude_estimate_constructor() { +static void test_estimate_constructor() { gnc::AttitudeEstimate estimate; TEST_ASSERT_TRUE(lin::all(lin::isnan(estimate.q_body_eci))); @@ -38,11 +46,80 @@ static void test_attitude_estimate_constructor() { TEST_ASSERT_FALSE(estimate.is_valid); } +static void test_simple_reset() { + lin::internal::RandomsGenerator randoms; + double t = randoms.next(); + lin::Vector4d q = lin::rands(4, 1, randoms); + q = q / lin::norm(q); + + gnc::AttitudeEstimatorState state; + + gnc::attitude_estimator_reset(state, t, q); + TEST_ASSERT_TRUE(state.is_valid); + TEST_ASSERT_DOUBLE_WITHIN(1.0e-6, t, state.t); + TEST_ASSERT_LIN_NEAR_ABS(1.0e-5f, q, state.q); + + gnc::attitude_estimator_reset(state, gnc::constant::nan, q); + TEST_ASSERT_FALSE(state.is_valid); +} + +static void test_triad_reset() { + // TODO : Implement this + // https://github.com/pathfinder-for-autonomous-navigation/psim/issues/187 +} + +static void test_update() { + // Inputs tested with the MATLAB filter + double dt = 0.1; + lin::Vector3f w_body { -0.024638540907753, 0.027980520091267, 0.013940768560450 }; + lin::Vector3f b_body { 0.000002211719363 , 0.000018231168377, 0.000021294908704 }; + lin::Vector3f r_ecef { 1.0e6 * -6.690748340286341, 1.0e6 * -0.708202263245960, 1.0e6 * 0.012794219705281 }; + lin::Vector4f q_init { 0.214170254370675, 0.051976879477451, 0.033112924164660, 0.974850265628446 }; + float s_phi = -0.107464154886802, s_theta = 1.616925529843563; + lin::Vector3f s_body { std::sin(s_theta) * std::cos(s_phi), std::sin(s_theta) * std::sin(s_phi), std::cos(s_theta) }; + + // Initialize and ensure that state is valid + gnc::AttitudeEstimatorState state; + gnc::attitude_estimator_reset(state, 0.0, q_init); + TEST_ASSERT_TRUE(state.is_valid); + + // Set sensor readings + gnc::AttitudeEstimatorData data; + data.t = dt; + data.r_ecef = r_ecef; + data.b_body = b_body; + data.s_body = s_body; + data.w_body = w_body; + + gnc::AttitudeEstimate estimate; + gnc::attitude_estimator_update(state, data, estimate); + + // Expected output from the MATLAB filter + lin::Vector4f q_out { 0.265608781930572, -0.009399967430439, 0.006565616837399, 0.964012711663705 }; + lin::Vector3f gyr_bias_out { -0.001776934793010, 0.001719572990915, 0.000684384595001 }; + lin::Matrixf<6, 6> P_out { + 0.000962383949459, 0.000027917201409, 0.000114565345871, 0.000003854083289, -0.000001996036030, -0.000000543690559, + 0.000027917201409, 0.000563006871950, 0.000441568073017, -0.000000422251875, 0.000002164652771, -0.000013540336585, + 0.000114565345871, 0.000441568073017, 0.000931209202754, -0.000001798696039, -0.000014780029384, -0.000008249390792, + 0.000003854083289, -0.000000422251875, -0.000001798696039, 0.004891748106294, 0.000000032435230, 0.000000007656056, + -0.000001996036030, 0.000002164652771, -0.000014780029384, 0.000000032435230, 0.004891799074639, 0.000000427300705, + -0.000000543690559, -0.000013540336585, -0.000008249390792, 0.000000007656056, 0.000000427300705, 0.004892094486247 + }; + TEST_ASSERT_TRUE(estimate.is_valid); + TEST_ASSERT_TRUE(state.is_valid); + TEST_ASSERT_LIN_FRO_NEAR_REL(1.0e-4f, q_out, estimate.q_body_eci); + TEST_ASSERT_LIN_FRO_NEAR_REL(1.0e-3f, gyr_bias_out, estimate.gyro_bias); + TEST_ASSERT_LIN_FRO_NEAR_REL(1.0e-2f, P_out, estimate.P); // Makes sense this would be the least accurate +} + static int test() { UNITY_BEGIN(); - RUN_TEST(test_attitude_estimator_state_constructor); - RUN_TEST(test_attitude_estimator_data_constructor); - RUN_TEST(test_attitude_estimate_constructor); + RUN_TEST(test_state_constructor); + RUN_TEST(test_data_constructor); + RUN_TEST(test_estimate_constructor); + RUN_TEST(test_simple_reset); + RUN_TEST(test_triad_reset); + RUN_TEST(test_update); return UNITY_END(); }