Merge pull request #227 from lasp/feature/852-simple-nav-gyro

Feature/852 simple nav gyro
lasp · Jun 18, 2024 · d2edcdc · d2edcdc
2 parents a2c2afc + 126d4a4
commit d2edcdc
Show file tree

Hide file tree

Showing 5 changed files with 119 additions and 6 deletions.
diff --git a/src/simulation/navigation/simpleNav/_UnitTest/test_simpleNav.py b/src/simulation/navigation/simpleNav/_UnitTest/test_simpleNav.py
@@ -1,6 +1,6 @@
 # ISC License
 #
-# Copyright (c) 2023, Laboratory for Atmospheric and Space Physics, CU Boulder
+# Copyright (c) 2024, University of Colorado at Boulder
 #
 # Permission to use, copy, modify, and/or distribute this software for any
 # purpose with or without fee is hereby granted, provided that the above
@@ -25,7 +25,7 @@
 
 
 def test_long_sim(show_plots):
-    """This unit test integrates the spacecraft motion (changing attutide/rate and postition/velocity)
+    """This unit test integrates the spacecraft motion (changing attitude/rate and position/velocity)
     and ensures that the simple navigation output is consistent with the noise inputs"""
     simple_nav_sim(show_plots, noise=True, number_steps=1000)
 
@@ -68,13 +68,15 @@ def simple_nav_sim(show_plots, noise=False, number_steps=1, sigma_test=3):
     vehicle_velocity = [2.0, 3.0, 1.0]
     vehicle_attitude = [0.3, 0.1, 0.1]
     vehicle_rate = [0.01, 0.02, 0.03]
+    vehicle_accel = [-0.0005, -0.0001, 0.0003]
     sun_heading_B = np.array(sun_position) - np.array(vehicle_position)
     sun_heading_B = np.dot(rbk.MRP2C(vehicle_attitude), sun_heading_B / np.linalg.norm(sun_heading_B))
 
     state_message.r_BN_N = vehicle_position
     state_message.v_BN_N = vehicle_velocity
     state_message.sigma_BN = vehicle_attitude
     state_message.omega_BN_B = vehicle_rate
+    state_message.omegaDot_BN_B = vehicle_accel
     spice_message.PositionVector = sun_position
     spice_message.PlanetName = "sun"
 
@@ -134,13 +136,22 @@ def simple_nav_sim(show_plots, noise=False, number_steps=1, sigma_test=3):
     simple_nav_object.crossTrans = True
     simple_nav_object.crossAtt = False
 
+    simple_nav_object.numberOfGyroBuffers = 100
+    simple_nav_object.gyroFrequencyPerSecond = 1000
+    simple_nav_object.gyroBias = 0.1
+    simple_nav_object.gyroStandardDeviation = 0.01
+    simple_nav_object.accelBias = 0.02
+    simple_nav_object.accelStandardDeviation = 0.002
+
     # setup logging
     rotational_data_log = simple_nav_object.attOutMsg.recorder()
     translational_data_log = simple_nav_object.transOutMsg.recorder()
     ephemeris_data_log = simple_nav_object.scEphemOutMsg.recorder()
+    accel_data_log = simple_nav_object.accelDataOutMsg.recorder()
     unit_test_sim.AddModelToTask(unit_task_name, rotational_data_log)
     unit_test_sim.AddModelToTask(unit_task_name, translational_data_log)
     unit_test_sim.AddModelToTask(unit_task_name, ephemeris_data_log)
+    unit_test_sim.AddModelToTask(unit_task_name, accel_data_log)
 
     unit_test_sim.InitializeSimulation()
     unit_test_sim.ConfigureStopTime(int(number_steps * 1E8))
@@ -160,6 +171,57 @@ def simple_nav_sim(show_plots, noise=False, number_steps=1, sigma_test=3):
     ephemeris_attitude_output = ephemeris_data_log.sigma_BN
     ephemeris_rate_output = ephemeris_data_log.omega_BN_B
 
+    test_packets = accel_data_log.accPkts
+    rate_array = np.zeros([number_steps*simple_nav_object.numberOfGyroBuffers, 3])
+    accel_array = np.zeros([number_steps*simple_nav_object.numberOfGyroBuffers, 3])
+    time_list = []
+    for i in range(number_steps+1):
+        for j in range(simple_nav_object.numberOfGyroBuffers):
+            time_list.append(test_packets[i]["accPkts.measTime"] - test_packets[0]["accPkts.measTime"])
+    for i in range(number_steps):
+        for j in range(simple_nav_object.numberOfGyroBuffers):
+            rate_array[i*simple_nav_object.numberOfGyroBuffers + j, :] = np.array(test_packets[i]["accPkts.gyro_B"])
+            accel_array[i*simple_nav_object.numberOfGyroBuffers + j, :] = np.array(test_packets[i]["accPkts.accel_B"])
+    avg_rate = np.mean(rate_array, axis=0)
+    avg_accel = np.mean(accel_array, axis=0)
+    avg_time = np.mean(time_list)
+    std_rate = np.std(rate_array, axis=0)
+    std_accel = np.std(accel_array, axis=0)
+
+    np.testing.assert_allclose(avg_time,
+                               (number_steps * 1E8)/2,
+                               atol=4*simple_nav_object.gyroStandardDeviation/np.sqrt(number_steps),
+                               equal_nan=False,
+                               err_msg="Average of the time measurements was not at middle of time range",
+                               verbose=True)
+    np.testing.assert_allclose(avg_rate,
+                               np.array(vehicle_rate) + np.array([simple_nav_object.gyroBias]*3),
+                               atol=4*simple_nav_object.gyroStandardDeviation/np.sqrt(number_steps),
+                               equal_nan=False,
+                               err_msg="Average accelerometer rate was not within 4 std deviations of mean + bias",
+                               verbose=True)
+    np.testing.assert_allclose(avg_accel,
+                               np.array(vehicle_accel) + np.array([simple_nav_object.accelBias]*3),
+                               atol=4*simple_nav_object.accelStandardDeviation/np.sqrt(number_steps),
+                               equal_nan=False,
+                               err_msg="Average accelerometer acceleration was not within 4 std deviations of "
+                                       "mean + bias",
+                               verbose=True)
+    np.testing.assert_allclose(std_rate,
+                               simple_nav_object.gyroStandardDeviation,
+                               atol=4*simple_nav_object.gyroStandardDeviation/np.sqrt(number_steps),
+                               equal_nan=False,
+                               err_msg="Standard deviation applied to gyros was not within 4 std deviations of "
+                                       "mean + bias",
+                               verbose=True)
+    np.testing.assert_allclose(std_accel,
+                               simple_nav_object.accelStandardDeviation,
+                               atol=4*simple_nav_object.accelStandardDeviation/np.sqrt(number_steps),
+                               equal_nan=False,
+                               err_msg="Standard deviation to accelerations was not within 4 std deviations of "
+                                       "mean + bias",
+                               verbose=True)
+
     # If the sigma is non-zero ensure errors are below the expected bounds, if not ensure errors are large enough
     if sigma_test != 0:
         np.testing.assert_allclose(ephemeris_position_output[number_steps, :],

diff --git a/src/simulation/navigation/simpleNav/simpleNav.cpp b/src/simulation/navigation/simpleNav/simpleNav.cpp
@@ -1,7 +1,7 @@
 /*
  ISC License
 
- Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
+ Copyright (c) 2024, University of Colorado at Boulder
 
  Permission to use, copy, modify, and/or distribute this software for any
  purpose with or without fee is hereby granted, provided that the above
@@ -35,6 +35,7 @@ SimpleNav::SimpleNav()
     this->trueAttState = this->attOutMsg.zeroMsgPayload;
     this->estTransState = this->transOutMsg.zeroMsgPayload;
     this->trueTransState = this->transOutMsg.zeroMsgPayload;
+    this->accelDataState = this->accelDataOutMsg.zeroMsgPayload;
     this->spacecraftEphemerisState = this->scEphemOutMsg.zeroMsgPayload;
     this->PMatrix.resize(18,18);
     this->PMatrix.fill(0.0);
@@ -88,9 +89,10 @@ void SimpleNav::Reset(uint64_t CurrentSimNanos)
         bskLogger.bskLog(BSK_ERROR, "Your walkbounds vector  is not 18 elements. Quitting");
     }
     this->errorModel.setUpperBounds(this->walkBounds);
+    vSetZero(this->gyroErrors, 3 * MAX_ACC_BUF_PKT);
+    vSetZero(this->accelErrors, 3 * MAX_ACC_BUF_PKT);
 }
 
-
 /*! This method reads the input messages associated with the vehicle state and
  the sun state
  */
@@ -119,6 +121,7 @@ void SimpleNav::writeOutputMessages(uint64_t Clock)
     this->attOutMsg.write(&this->estAttState, this->moduleID, Clock);
     this->transOutMsg.write(&this->estTransState, this->moduleID, Clock);
     this->scEphemOutMsg.write(&this->spacecraftEphemerisState, this->moduleID, Clock);
+    this->accelDataOutMsg.write(&this->accelDataState, this->moduleID, Clock);
 }
 
 void SimpleNav::applyErrors()
@@ -137,6 +140,15 @@ void SimpleNav::applyErrors()
     v3Add(this->spacecraftEphemerisState.omega_BN_B, &(this->navErrors.data()[9]),
           this->spacecraftEphemerisState.omega_BN_B);
     v3Add(this->trueTransState.vehAccumDV, &(this->navErrors.data()[15]), this->estTransState.vehAccumDV);
+
+    //! - Apply accelerometer errors to truth data
+    for (int index=0; index<this->numberOfGyroBuffers; ++index){
+        AccPktDataMsgPayload accelPacketPayload = this->accelDataState.accPkts[index];
+        v3Add(accelPacketPayload.gyro_B, &this->gyroErrors[3*index], accelPacketPayload.gyro_B);
+        v3Add(accelPacketPayload.accel_B, &this->accelErrors[3*index], accelPacketPayload.accel_B);
+        this->accelDataState.accPkts[index] = accelPacketPayload;
+    }
+
     //! - Add errors to  sun-pointing
     if(this->sunStateInMsg.isLinked()){
         double dcm_OT[3][3];       /* dcm, body T to body O */
@@ -163,6 +175,16 @@ void SimpleNav::computeTrueOutput(uint64_t Clock)
     v3Copy(this->inertialState.omega_BN_B, this->trueAttState.omega_BN_B);
     v3Copy(this->inertialState.TotalAccumDVBdy, this->trueTransState.vehAccumDV);
 
+    //! - Set accelerometer state to truth data
+    for (int index=0; index<this->numberOfGyroBuffers; ++index){
+        AccPktDataMsgPayload accelPacketPayload;
+        uint64_t timeOffset = index*SEC2NANO/this->gyroFrequencyPerSecond;
+        accelPacketPayload.measTime = Clock + timeOffset;
+        v3Copy(this->inertialState.omega_BN_B, accelPacketPayload.gyro_B);
+        v3Copy(this->inertialState.omegaDot_BN_B, accelPacketPayload.accel_B);
+        this->accelDataState.accPkts[index] = accelPacketPayload;
+    }
+
     //! - Set ephemeris state to truth data
     v3Copy(this->inertialState.r_BN_N, this->spacecraftEphemerisState.r_BdyZero_N);
     v3Copy(this->inertialState.v_BN_N, this->spacecraftEphemerisState.v_BdyZero_N);
@@ -206,6 +228,17 @@ void SimpleNav::computeErrors(uint64_t CurrentSimNanos)
     this->errorModel.setPropMatrix(localProp);
     this->errorModel.computeNextState();
     this->navErrors = this->errorModel.getCurrentState();
+
+    //! - Compute accelerometer errors
+    std::random_device rd;
+    std::mt19937 generator(rd());
+    std::normal_distribution<double> gyroErrorDistribution(this->gyroBias,this->gyroStandardDeviation);
+    std::normal_distribution<double> accelErrorDistribution(this->accelBias,this->accelStandardDeviation);
+    for (int index=0; index<3*this->numberOfGyroBuffers; ++index){
+        this->gyroErrors[index] = gyroErrorDistribution(generator);
+        this->accelErrors[index] = accelErrorDistribution(generator);
+    }
+
 }
 
 /*! This method calls all of the run-time operations for the simple nav model.

diff --git a/src/simulation/navigation/simpleNav/simpleNav.h b/src/simulation/navigation/simpleNav/simpleNav.h
@@ -1,7 +1,7 @@
 /*
  ISC License
 
- Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
+ Copyright (c) 2024, University of Colorado at Boulder
 
  Permission to use, copy, modify, and/or distribute this software for any
  purpose with or without fee is hereby granted, provided that the above
@@ -21,13 +21,16 @@
 #define SIMPLE_NAV_H
 
 #include <vector>
+#include <random>
 #include "architecture/_GeneralModuleFiles/sys_model.h"
 #include "architecture/utilities/gauss_markov.h"
 #include "architecture/msgPayloadDefC/SCStatesMsgPayload.h"
 #include "architecture/msgPayloadDefC/SpicePlanetStateMsgPayload.h"
 #include "architecture/msgPayloadDefC/NavAttMsgPayload.h"
 #include "architecture/msgPayloadDefC/NavTransMsgPayload.h"
 #include "architecture/msgPayloadDefC/EphemerisMsgPayload.h"
+#include "architecture/msgPayloadDefC/AccPktDataMsgPayload.h"
+#include "architecture/msgPayloadDefC/AccDataMsgPayload.h"
 #include "architecture/utilities/bskLogging.h"
 #include <Eigen/Dense>
 #include "architecture/messaging/messaging.h"
@@ -47,19 +50,29 @@ class SimpleNav: public SysModel {
     void writeOutputMessages(uint64_t Clock);
 
 public:
+    double gyroStandardDeviation=1E-5;    //!< Standard deviation for each rate component
+    double accelStandardDeviation=1E-8;    //!< Standard deviation for each acceleration component
+    double gyroBias=0 ;    //!<  Bias for each rate component
+    double accelBias=0;    //!<  Bias for each acceleration component
+    double gyroErrors[3*MAX_ACC_BUF_PKT]; //!<  Errors to apply to each gyro measurement
+    double accelErrors[3*MAX_ACC_BUF_PKT]; //!<  Errors to apply to each accelerometer measurement
+    int numberOfGyroBuffers=100;       //!< Number of gyro measurements per timestep
+    int gyroFrequencyPerSecond=500;       //!< Number of gyro measurements per second
     Eigen::MatrixXd PMatrix;          //!< -- Cholesky-decomposition or matrix square root of the covariance matrix to apply errors with
     Eigen::VectorXd walkBounds;       //!< -- "3-sigma" errors to permit for states
     Eigen::VectorXd navErrors;        //!< -- Current navigation errors applied to truth
     Message<NavAttMsgPayload> attOutMsg;        //!< attitude navigation output msg
     Message<NavTransMsgPayload> transOutMsg;    //!< translation navigation output msg
     Message<EphemerisMsgPayload> scEphemOutMsg;    //!< translation navigation output msg
+    Message<AccDataMsgPayload> accelDataOutMsg; //!< accelerometer and gyro data output msg
     bool crossTrans;                  //!< -- Have position error depend on velocity
     bool crossAtt;                    //!< -- Have attitude depend on attitude rate
     NavAttMsgPayload trueAttState;        //!< -- attitude nav state without errors
     NavAttMsgPayload estAttState;         //!< -- attitude nav state including errors
     NavTransMsgPayload trueTransState;    //!< -- translation nav state without errors
     NavTransMsgPayload estTransState;     //!< -- translation nav state including errors
     EphemerisMsgPayload spacecraftEphemerisState;    //!< -- full spacecraft ephemeris state with errors
+    AccDataMsgPayload accelDataState;  //!< accelerometer and gyro data payload
     SCStatesMsgPayload inertialState; //!< -- input inertial state from Star Tracker
     SpicePlanetStateMsgPayload sunState;  //!< -- input Sun state
     BSKLogger bskLogger;              //!< -- BSK Logging

diff --git a/src/simulation/navigation/simpleNav/simpleNav.i b/src/simulation/navigation/simpleNav/simpleNav.i
@@ -1,7 +1,7 @@
 /*
  ISC License
 
- Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
+ Copyright (c) 2024, University of Colorado at Boulder
 
  Permission to use, copy, modify, and/or distribute this software for any
  purpose with or without fee is hereby granted, provided that the above
@@ -41,6 +41,8 @@ struct NavTransMsg_C;
 struct SpicePlanetStateMsg_C;
 %include "architecture/msgPayloadDefC/EphemerisMsgPayload.h"
 struct EphemerisMsg_C;
+%include "architecture/msgPayloadDefC/AccDataMsgPayload.h"
+struct AccDataMsg_C;
 
 %pythoncode %{
 import sys

diff --git a/src/simulation/navigation/simpleNav/simpleNav.rst b/src/simulation/navigation/simpleNav/simpleNav.rst
@@ -38,6 +38,9 @@ not come from a noisy sensor but from a ground-based estimation process.
     * - scEphemOutMsg
       - :ref:`EphemerisMsgPayload`
       - spacecraft ephemeris output msg
+    * - scEphemOutMsg
+      - :ref:`accelDataOutMsg`
+      - spacecraft accelerometer and gyro output msg
     * - scStateInMsg
       - :ref:`SCStatesMsgPayload`
       - spacecraft state input msg