Updated the attitude estimator flight computer model

include/psim/fc/attitude_estimator.hpp

@@ -55,9 +55,12 @@ class AttitudeEstimator
   virtual void add_fields(State &state) override;
   virtual void step() override;
 
+  Vector4 fc_satellite_attitude_q_body_eci_error() const;
+  Real fc_satellite_atttitude_q_body_eci_error_degrees() const;
+  Vector3 fc_satellite_attitude_p_body_eci_error() const;
+  Vector3 fc_satellite_attitude_p_body_eci_sigma() const;
+  Vector3 fc_satellite_attitude_w_bias_error() const;
   Vector3 fc_satellite_attitude_w_bias_sigma() const;
-  Real fc_satellite_attitude_P_fro() const;
-  Real fc_satellite_attitude_log_P_fro() const;
 };
 } // namespace psim
 

include/psim/fc/attitude_estimator.yml

@@ -7,7 +7,7 @@ comment: >
 
 args:
     - satellite
-    - orbit
+    - position
 
 adds:
     - name: "fc.{satellite}.attitude.is_valid"
@@ -16,32 +16,52 @@ adds:
         Flag specifying whether or not the estimator is currently initialized.
         Zero indicated the estimator isn't initialized while one indicates it
         has been initialized.
+
     - name: "fc.{satellite}.attitude.q.body_eci"
       type: Vector4
       comment: >
-        Current attitude estimate for the satellite. This value is represented
-        as a quaternion.
+        Current attitude estimate.
+    - name: "fc.{satellite}.attitude.q.body_eci.error"
+      type: Lazy Vector4
+      comment: >
+        Current attitude estimate error.
+    - name: "fc.{satellite}.attitude.q.body_eci.error.degrees"
+      type: Lazy Real
+      comment: >
+        Current attitude estimate error represented as a scalar in degrees.
+
+    - name: "fc.{satellite}.attitude.p.body_eci.error"
+      type: Lazy Vector3
+      comment: >
+        Current attitude estimate error. This value is represented as a
+        Generalized Rodrigues Parameter.
+    - name: "fc.{satellite}.attitude.p.body_eci.sigma"
+      type: Lazy Vector3
+      comment: >
+        One sigma bounds on the current attitude estimate error. This value is
+        represented as a Generalized Rodrigues Parameter.
+
     - name: "fc.{satellite}.attitude.w.bias"
       type: Vector3
       comment: >
-        Current angular rate bias estimate for the satellite.
-    - name: "fc.{satellite}.attitude.w.bias.sigma"
+        Current angular rate bias estimate.
+    - name: "fc.{satellite}.attitude.w.bias.error"
       type: Lazy Vector3
       comment: >
-        One sigma bounds on the estimate of the gyroscope bias.
-    - name: "fc.{satellite}.attitude.P_fro"
-      type: Lazy Real
-      comment: >
-        Frobenius norm of the estimate's state covariance.
-    - name: "fc.{satellite}.attitude.log_P_fro"
-      type: Lazy Real
+        Current angular rate bias estimate error.
+    - name: "fc.{satellite}.attitude.w.bias.sigma"
+      type: Lazy Vector3
       comment: >
-        Log of the Frobenius norm of the estimate's state covariance.
+        One sigma bounds on the current angular rate bias estimate error.
 
 gets:
     - name: "truth.t.s"
       type: Real
-    - name: "{orbit}.r.ecef"
+    - name: "truth.{satellite}.attitude.q.eci_body"
+      type: Vector4
+    - name: "sensors.{satellite}.gyroscope.w.bias"
+      type: Vector3
+    - name: "{position}"
       type: Vector3
     - name: "sensors.{satellite}.gyroscope.w"
       type: Vector3

src/psim/fc/attitude_estimator.cpp

@@ -28,8 +28,11 @@
 
 #include <psim/fc/attitude_estimator.hpp>
 
+#include <gnc/utilities.hpp>
+
 #include <lin/math.hpp>
 #include <lin/queries.hpp>
+#include <lin/references.hpp>
 
 namespace psim {
 
@@ -51,7 +54,7 @@ void AttitudeEstimator::step() {
   this->Super::step();
 
   auto const &t = truth_t_s->get();
-  auto const &r = orbit_r_ecef->get();
+  auto const &r = position->get();
   auto const &b = sensors_satellite_magnetometer_b->get();
   auto const &s = sensors_satellite_sun_sensors_s->get();
   auto const &w = sensors_satellite_gyroscope_w->get();
@@ -79,19 +82,48 @@ void AttitudeEstimator::step() {
   _set_attitude_outputs();
 }
 
-Vector3 AttitudeEstimator::fc_satellite_attitude_w_bias_sigma() const {
+Vector4 AttitudeEstimator::fc_satellite_attitude_q_body_eci_error() const {
+  auto const &truth_q_eci_body = truth_satellite_attitude_q_eci_body->get();
+  auto const &q_body_eci = Super::fc_satellite_attitude_q_body_eci.get();
+
+  Vector4 q_error;
+  gnc::utl::quat_cross_mult(q_body_eci, truth_q_eci_body, q_error);
+  return q_error;
+}
+
+Real AttitudeEstimator::fc_satellite_atttitude_q_body_eci_error_degrees() const {
+  auto const &q_error = Super::fc_satellite_attitude_q_body_eci_error.get();
+
+  return 2.0 * lin::asin(lin::norm(lin::ref<3, 1>(q_error, 0, 0)));
+}
+
+Vector3 AttitudeEstimator::fc_satellite_attitude_p_body_eci_error() const {
+  auto const &q_error = Super::fc_satellite_attitude_q_body_eci_error.get();
+
+  constexpr static Real a = 1.0;
+  constexpr static Real f = 2.0 * (a + 1.0);
+
+  Vector3 p_error;
+  gnc::utl::quat_to_qrp(q_error, a, f, p_error);
+  return p_error;
+}
+
+Vector3 AttitudeEstimator::fc_satellite_attitude_p_body_eci_sigma() const {
   auto const &P = _attitude_state.P;
 
-  return {lin::sqrt(P(3, 3)), lin::sqrt(P(4, 4)), lin::sqrt(P(5, 5))};
+  return {lin::sqrt(P(0, 0)), lin::sqrt(P(1, 1)), lin::sqrt(P(2, 2))};
 }
 
-Real AttitudeEstimator::fc_satellite_attitude_P_fro() const {
-  return lin::fro(_attitude_state.P);
+Vector3 AttitudeEstimator::fc_satellite_attitude_w_bias_error() const {
+  auto const &truth_bias = sensors_satellite_gyroscope_w_bias->get();
+  auto const &bias = fc_satellite_attitude_w_bias.get();
+
+  return truth_bias - bias;
 }
 
-Real AttitudeEstimator::fc_satellite_attitude_log_P_fro() const {
-  auto const &P_fro = this->Super::fc_satellite_attitude_P_fro.get();
+Vector3 AttitudeEstimator::fc_satellite_attitude_w_bias_sigma() const {
+  auto const &P = _attitude_state.P;
 
-  return lin::log10(P_fro);
+  return {lin::sqrt(P(3, 3)), lin::sqrt(P(4, 4)), lin::sqrt(P(5, 5))};
 }
 } // namespace psim