From db5616c244bb486d54c7eac8bdcda84806d41d18 Mon Sep 17 00:00:00 2001
From: Leah Kiner <leahannkiner@gmail.com>
Date: Fri, 27 Sep 2024 16:58:45 -0600
Subject: [PATCH 1/5] Add timeTag and numberOfValidGyroMeasurements to
IMUSensorMsgPayload
---
src/architecture/msgPayloadDefC/IMUSensorMsgPayload.h | 2 ++
1 file changed, 2 insertions(+)
diff --git a/src/architecture/msgPayloadDefC/IMUSensorMsgPayload.h b/src/architecture/msgPayloadDefC/IMUSensorMsgPayload.h
index fc0b1e19e9..3b72cd8ddb 100755
--- a/src/architecture/msgPayloadDefC/IMUSensorMsgPayload.h
+++ b/src/architecture/msgPayloadDefC/IMUSensorMsgPayload.h
@@ -27,6 +27,8 @@ typedef struct{
double AccelPlatform[3]; //!< m/s2 Apparent acceleration of the platform
double DRFramePlatform[3]; //!< r Accumulated DRs in platform
double AngVelPlatform[3]; //!< r/s Angular velocity in platform frame
+ double timeTag; //!< [-] Observation time tag
+ double numberOfValidGyroMeasurements; //!< Number of valid gyro measurements
}IMUSensorMsgPayload;
From 8fe2fd8c2b775d7fadee5ab978b8325ba9819804 Mon Sep 17 00:00:00 2001
From: Leah Kiner <leahannkiner@gmail.com>
Date: Fri, 27 Sep 2024 16:59:00 -0600
Subject: [PATCH 2/5] Update inertial attitude ukf to accept
IMUSensorMsgPayload
---
.../inertialAttitudeUkf.cpp | 39 +++----------------
.../inertialAttitudeUkf/inertialAttitudeUkf.h | 4 +-
.../inertialAttitudeUkf/inertialAttitudeUkf.i | 6 +--
3 files changed, 10 insertions(+), 39 deletions(-)
diff --git a/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.cpp b/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.cpp
index 8f3467a353..cf9666bbdf 100644
--- a/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.cpp
+++ b/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.cpp
@@ -170,43 +170,16 @@ void InertialAttitudeUkf::readStarTrackerData(){
* @return void
* */
void InertialAttitudeUkf::readGyroData(){
- int smallestFutureIndex = 0;
- int numberOfValidGyroMeasurements = 0;
- double firstFutureTime = -1;
- double meanMeasurementTime = 0;
- AccDataMsgPayload gyrBuffer = this->accelDataMsg();
- for (int index = 0; index < MAX_ACC_BUF_PKT; index++) {
- double gyroMeasuredTime = gyrBuffer.accPkts[index].measTime*NANO2SEC;
- if (gyroMeasuredTime > this->previousFilterTimeTag) {
- if (gyroMeasuredTime < firstFutureTime || firstFutureTime<0){
- smallestFutureIndex = index;
- firstFutureTime = gyroMeasuredTime;
- }
- meanMeasurementTime += gyroMeasuredTime;
- numberOfValidGyroMeasurements += 1;
- }
- }
- auto lowPass = LowPassFilter();
- lowPass.setFilterCutoff(this->cutOffFrequency);
- lowPass.setFilterStep(this->hStep);
- if (numberOfValidGyroMeasurements > 0){
- meanMeasurementTime /= numberOfValidGyroMeasurements;
- /*! - Loop through buffer for all future measurements since the previous time to filter omega_BN_B*/
- for (int index = 0; index < MAX_ACC_BUF_PKT; index++) {
- int shiftedIndex = (index + smallestFutureIndex) % MAX_ACC_BUF_PKT;
- auto omega_BN_B = Eigen::Map<Eigen::Vector3d>(gyrBuffer.accPkts[shiftedIndex].gyro_B);
- /*! - Apply low-pass filter to gyro measurements to get smoothed body rate*/
- lowPass.processMeasurement(omega_BN_B);
- }
-
+ IMUSensorMsgPayload gyroBuffer = this->imuSensorDataInMsg();
+ if (gyroBuffer.timeTag*NANO2SEC > this->previousFilterTimeTag) {
auto gyroMeasurement = MeasurementModel();
gyroMeasurement.setMeasurementName("gyro");
- gyroMeasurement.setTimeTag(meanMeasurementTime);
+ gyroMeasurement.setTimeTag(gyroBuffer.timeTag);
gyroMeasurement.setValidity(true);
gyroMeasurement.setMeasurementNoise(
- this->measNoiseScaling * this->gyroNoise/std::sqrt(numberOfValidGyroMeasurements));
- gyroMeasurement.setObservation(lowPass.getCurrentState());
+ this->measNoiseScaling * this->gyroNoise/std::sqrt(gyroBuffer.numberOfValidGyroMeasurements));
+ gyroMeasurement.setObservation(cArray2EigenVector3d(gyroBuffer.AngVelPlatform));
gyroMeasurement.setMeasurementModel(MeasurementModel::velocityStates);
this->measurements[this->measurementIndex] = gyroMeasurement;
this->measurementIndex += 1;
@@ -249,7 +222,7 @@ void InertialAttitudeUkf::setGyroNoise(const Eigen::Matrix3d &gyroNoiseInput) {
this->gyroNoise = gyroNoiseInput;
}
-/*! Get he gyro measurement noise matrix
+/*! Get the gyro measurement noise matrix
@return Eigen::Matrix3d gyroNoise
*/
Eigen::Matrix3d InertialAttitudeUkf::getGyroNoise() const {
diff --git a/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.h b/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.h
index b2cebcf742..70b9219ff5 100644
--- a/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.h
+++ b/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.h
@@ -36,7 +36,7 @@
#include "architecture/msgPayloadDefCpp/FilterResidualsMsgPayload.h"
#include "architecture/msgPayloadDefC/STAttMsgPayload.h"
-#include "architecture/msgPayloadDefC/AccDataMsgPayload.h"
+#include "architecture/msgPayloadDefC/IMUSensorMsgPayload.h"
#include "architecture/msgPayloadDefC/RWSpeedMsgPayload.h"
#include "architecture/msgPayloadDefC/VehicleConfigMsgPayload.h"
#include "architecture/msgPayloadDefC/RWArrayConfigMsgPayload.h"
@@ -77,7 +77,7 @@ class InertialAttitudeUkf: public SRukfInterface {
RWArrayConfigMsgPayload rwArrayConfigPayload;
ReadFunctor<VehicleConfigMsgPayload> vehicleConfigMsg;
ReadFunctor<RWSpeedMsgPayload> rwSpeedMsg;
- ReadFunctor<AccDataMsgPayload> accelDataMsg;
+ ReadFunctor<IMUSensorMsgPayload> imuSensorDataInMsg;
Message<NavAttMsgPayload> navAttitudeOutputMsg;
Message<FilterMsgPayload> inertialFilterOutputMsg;
diff --git a/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.i b/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.i
index 8454c0ab77..8342b88996 100644
--- a/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.i
+++ b/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.i
@@ -41,10 +41,8 @@ struct VehicleConfigMsg_C;
struct RWArrayConfigMsg_C;
%include "architecture/msgPayloadDefC/RWSpeedMsgPayload.h"
struct RWSpeedMsg_C;
-%include "architecture/msgPayloadDefC/AccDataMsgPayload.h"
-struct AccDataMsg_C;
-%include "architecture/msgPayloadDefC/AccPktDataMsgPayload.h"
-struct AccPktDataMsg_C;
+%include "architecture/msgPayloadDefC/IMUSensorMsgPayload.h"
+struct IMUSensorMsg_C;
%include "architecture/msgPayloadDefC/NavAttMsgPayload.h"
struct NavAttMsg_C;
From 0bc9f8468a8e878e2b84096c022cabeb56d38014 Mon Sep 17 00:00:00 2001
From: Leah Kiner <leahannkiner@gmail.com>
Date: Mon, 30 Sep 2024 15:25:08 -0600
Subject: [PATCH 3/5] Add miruLowPassFilterConverter to sensorInterfaces
---
.../miruLowPassFilterConverter.cpp | 63 +++++++++++++++++++
.../miruLowPassFilterConverter.h | 33 ++++++++++
.../miruLowPassFilterConverter.i | 21 +++++++
3 files changed, 117 insertions(+)
create mode 100755 src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.cpp
create mode 100755 src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.h
create mode 100755 src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.i
diff --git a/src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.cpp b/src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.cpp
new file mode 100755
index 0000000000..ec93596be2
--- /dev/null
+++ b/src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.cpp
@@ -0,0 +1,63 @@
+#include "miruLowPassFilterConverter.h"
+#include "architecture/utilities/avsEigenSupport.h"
+
+/*! This method checks the input message to ensure it is linked.
+ @return void
+ @param callTime [ns] Time the method is called
+*/
+void MiruLowPassFilterConverter::Reset(uint64_t callTime) {
+ if (!this->imuAccelDataInMsg.isLinked()) {
+ this->bskLogger->bskLog(BSK_ERROR, "miruLowPassFilterConverter.imuAccelDataInMsg wasn't connected.");
+ }
+}
+
+/*! This method writes the module output message using the given input message.
+ @return void
+ @param callTime [ns] Time the method is called
+*/
+void MiruLowPassFilterConverter::UpdateState(uint64_t callTime) {
+ auto lowPass = LowPassFilter();
+ int smallestFutureIndex = 0;
+ int numberOfValidGyroMeasurements = 0;
+ double firstFutureTime = -1;
+ double meanMeasurementTime = 0;
+ AccDataMsgPayload gyrBuffer = this->imuAccelDataInMsg();
+ for (int index = 0; index < MAX_ACC_BUF_PKT; index++) {
+ double gyroMeasuredTime = gyrBuffer.accPkts[index].measTime*NANO2SEC;
+ if (gyroMeasuredTime < firstFutureTime || firstFutureTime<0){
+ smallestFutureIndex = index;
+ firstFutureTime = gyroMeasuredTime;
+ }
+ meanMeasurementTime += gyroMeasuredTime;
+ numberOfValidGyroMeasurements += 1;
+ }
+ lowPass.setFilterCutoff(this->cutOffFrequency);
+ lowPass.setFilterStep(this->hStep);
+ if (numberOfValidGyroMeasurements > 0){
+ meanMeasurementTime /= numberOfValidGyroMeasurements;
+ /*! - Loop through buffer for all future measurements since the previous time to filter omega_BN_B*/
+ for (int index = 0; index < MAX_ACC_BUF_PKT; index++) {
+ int shiftedIndex = (index + smallestFutureIndex) % MAX_ACC_BUF_PKT;
+ auto omega_BN_B = Eigen::Map<Eigen::Vector3d>(gyrBuffer.accPkts[shiftedIndex].gyro_B);
+ /*! - Apply low-pass filter to gyro measurements to get smoothed body rate*/
+ lowPass.processMeasurement(omega_BN_B);
+ }
+ }
+
+ // Write the output message
+ IMUSensorMsgPayload imuSensorOut = IMUSensorMsgPayload();
+ imuSensorOut.timeTag = meanMeasurementTime;
+ imuSensorOut.numberOfValidGyroMeasurements = numberOfValidGyroMeasurements;
+ Eigen::Vector3d omega_BN_B = lowPass.getCurrentState();
+ eigenVector3d2CArray(omega_BN_B, imuSensorOut.AngVelPlatform);
+ this->imuSensorOutMsg.write(&imuSensorOut, moduleID, callTime);
+}
+
+/*! Set the low pass filter parameters
+ @param double step
+ @param double frequencyCutOff
+*/
+void MiruLowPassFilterConverter::setLowPassFilter(double step, double frequencyCutOff) {
+ this->hStep = step;
+ this->cutOffFrequency = frequencyCutOff;
+}
diff --git a/src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.h b/src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.h
new file mode 100755
index 0000000000..350973b919
--- /dev/null
+++ b/src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.h
@@ -0,0 +1,33 @@
+#ifndef _MIRULOWPASSFILTERCONVERTER_
+#define _MIRULOWPASSFILTERCONVERTER_
+
+#include "architecture/_GeneralModuleFiles/sys_model.h"
+#include "architecture/messaging/messaging.h"
+#include "architecture/utilities/bskLogging.h"
+#include "architecture/utilities/macroDefinitions.h"
+#include "architecture/msgPayloadDefC/AccDataMsgPayload.h"
+#include "architecture/msgPayloadDefC/IMUSensorMsgPayload.h"
+#include "architecture/utilities/signalProcessing.h"
+#include <Eigen/Core>
+
+/*! @brief Convert AccDataMsgPayload to IMUSensorMsgPayload Class */
+class MiruLowPassFilterConverter: public SysModel {
+public:
+ MiruLowPassFilterConverter() = default; //!< Constructor
+ ~MiruLowPassFilterConverter() = default; //!< Destructor
+
+ void Reset(uint64_t CurrentSimNanos); //!< Reset member function
+ void UpdateState(uint64_t CurrentSimNanos); //!< Update member function
+ void setLowPassFilter(double step, double frequencyCutOff); //!< Setter method for the low pass filter
+
+ ReadFunctor<AccDataMsgPayload> imuAccelDataInMsg; //!< Input msg for the imu data
+ Message<IMUSensorMsgPayload> imuSensorOutMsg; //!< Output msg for the imu data
+
+ BSKLogger *bskLogger; //!< BSK Logging
+
+private:
+ double cutOffFrequency = 15.0/(2*M_PI); //!< Low pass filter parameter
+ double hStep = 0.5; //!< Low pass filter parameter
+};
+
+#endif
diff --git a/src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.i b/src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.i
new file mode 100755
index 0000000000..14a5637085
--- /dev/null
+++ b/src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.i
@@ -0,0 +1,21 @@
+%module miruLowPassFilterConverter
+%{
+ #include "miruLowPassFilterConverter.h"
+%}
+
+%pythoncode %{
+from Basilisk.architecture.swig_common_model import *
+%}
+%include "std_string.i"
+%include "swig_conly_data.i"
+
+%include "sys_model.i"
+%include "miruLowPassFilterConverter.h"
+
+%include "architecture/msgPayloadDefC/AccDataMsgPayload.h"
+%include "architecture/msgPayloadDefC/IMUSensorMsgPayload.h"
+
+%pythoncode %{
+import sys
+protectAllClasses(sys.modules[__name__])
+%}
From cd746a613ff35e3b75f697c9d6e3000f58aea6e5 Mon Sep 17 00:00:00 2001
From: Leah Kiner <leahannkiner@gmail.com>
Date: Mon, 30 Sep 2024 15:26:50 -0600
Subject: [PATCH 4/5] Update inertialAttitudeUkf unit test
---
.../tests/test_inertialAttitudeUkf.py | 17 +++++++++++++++--
1 file changed, 15 insertions(+), 2 deletions(-)
diff --git a/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/tests/test_inertialAttitudeUkf.py b/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/tests/test_inertialAttitudeUkf.py
index ead5a8be9a..ae66ea95e2 100644
--- a/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/tests/test_inertialAttitudeUkf.py
+++ b/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/tests/test_inertialAttitudeUkf.py
@@ -20,6 +20,7 @@
import pytest
from Basilisk.architecture import messaging
from Basilisk.fswAlgorithms import inertialAttitudeUkf
+from Basilisk.fswAlgorithms import miruLowPassFilterConverter
from Basilisk.utilities import SimulationBaseClass, macros
from Basilisk.utilities import RigidBodyKinematics as rbk
@@ -107,9 +108,15 @@ def test_propagation_kf(show_plots):
test_process = unit_test_sim.CreateNewProcess(unit_process_name)
test_process.addTask(unit_test_sim.CreateNewTask(unit_task_name, test_process_rate))
+ # Create miruLowPassFilterConverter module
+ miru_low_pass_filter_converter = miruLowPassFilterConverter.MiruLowPassFilterConverter()
+ miru_low_pass_filter_converter.setLowPassFilter(0.5, 15/(2*np.pi))
+ unit_test_sim.AddModelToTask(unit_task_name, miru_low_pass_filter_converter)
+
# Construct algorithm and associated C++ container
allMeasurements = inertialAttitudeUkf.AttitudeFilterMethod_AllMeasurements
intertialAttitudeFilter = inertialAttitudeUkf.InertialAttitudeUkf(allMeasurements)
+ intertialAttitudeFilter.imuSensorDataInMsg.subscribeTo(miru_low_pass_filter_converter.imuSensorOutMsg)
unit_test_sim.AddModelToTask(unit_task_name, intertialAttitudeFilter)
# Add test module to runtime call list
@@ -173,7 +180,7 @@ def test_propagation_kf(show_plots):
accel_data = messaging.AccDataMsgPayload()
accel_measurement = messaging.AccDataMsg().write(accel_data)
- intertialAttitudeFilter.accelDataMsg.subscribeTo(accel_measurement)
+ miru_low_pass_filter_converter.imuAccelDataInMsg.subscribeTo(accel_measurement)
attitude_data_log = intertialAttitudeFilter.inertialFilterOutputMsg.recorder()
unit_test_sim.AddModelToTask(unit_task_name, attitude_data_log)
@@ -226,7 +233,13 @@ def test_measurements_kf(show_plots, initial_error, method):
test_process = unit_test_sim.CreateNewProcess(unit_process_name)
test_process.addTask(unit_test_sim.CreateNewTask(unit_task_name, test_process_rate))
+ # Create miruLowPassFilterConverter module
+ miru_low_pass_filter_converter = miruLowPassFilterConverter.MiruLowPassFilterConverter()
+ miru_low_pass_filter_converter.setLowPassFilter(0.5, 15/(2*np.pi))
+ unit_test_sim.AddModelToTask(unit_task_name, miru_low_pass_filter_converter)
+
intertialAttitudeFilter = inertialAttitudeUkf.InertialAttitudeUkf(method)
+ intertialAttitudeFilter.imuSensorDataInMsg.subscribeTo(miru_low_pass_filter_converter.imuSensorOutMsg)
unit_test_sim.AddModelToTask(unit_task_name, intertialAttitudeFilter)
# Add test module to runtime call list
@@ -294,7 +307,7 @@ def test_measurements_kf(show_plots, initial_error, method):
accel_data = messaging.AccDataMsgPayload()
accel_measurement = messaging.AccDataMsg().write(accel_data)
- intertialAttitudeFilter.accelDataMsg.subscribeTo(accel_measurement)
+ miru_low_pass_filter_converter.imuAccelDataInMsg.subscribeTo(accel_measurement)
filter_data_log = intertialAttitudeFilter.inertialFilterOutputMsg.recorder()
unit_test_sim.AddModelToTask(unit_task_name, filter_data_log)
From 9fc0cac65600ac3ac39a0aa82cefb83a09b2402b Mon Sep 17 00:00:00 2001
From: Leah Kiner <leahannkiner@gmail.com>
Date: Mon, 3 Mar 2025 14:32:47 -0700
Subject: [PATCH 5/5] pre-commit changes
---
.../msgPayloadDefC/IMUSensorMsgPayload.h | 18 +--
.../inertialAttitudeUkf.cpp | 143 +++++++++---------
.../inertialAttitudeUkf/inertialAttitudeUkf.h | 46 +++---
.../miruLowPassFilterConverter.cpp | 7 +-
.../miruLowPassFilterConverter.h | 33 ++--
5 files changed, 121 insertions(+), 126 deletions(-)
mode change 100755 => 100644 src/architecture/msgPayloadDefC/IMUSensorMsgPayload.h
mode change 100755 => 100644 src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.cpp
mode change 100755 => 100644 src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.h
diff --git a/src/architecture/msgPayloadDefC/IMUSensorMsgPayload.h b/src/architecture/msgPayloadDefC/IMUSensorMsgPayload.h
old mode 100755
new mode 100644
index 3b72cd8ddb..6c5ef4c2c4
--- a/src/architecture/msgPayloadDefC/IMUSensorMsgPayload.h
+++ b/src/architecture/msgPayloadDefC/IMUSensorMsgPayload.h
@@ -20,16 +20,14 @@
#ifndef _IMU_SENSOR_MESSAG_H
#define _IMU_SENSOR_MESSAG_H
-
//! @brief Simulated IMU Sensor output message definition.
-typedef struct{
- double DVFramePlatform[3]; //!< m/s Accumulated DVs in platform
- double AccelPlatform[3]; //!< m/s2 Apparent acceleration of the platform
- double DRFramePlatform[3]; //!< r Accumulated DRs in platform
- double AngVelPlatform[3]; //!< r/s Angular velocity in platform frame
- double timeTag; //!< [-] Observation time tag
- double numberOfValidGyroMeasurements; //!< Number of valid gyro measurements
-}IMUSensorMsgPayload;
-
+typedef struct {
+ double DVFramePlatform[3]; //!< m/s Accumulated DVs in platform
+ double AccelPlatform[3]; //!< m/s2 Apparent acceleration of the platform
+ double DRFramePlatform[3]; //!< r Accumulated DRs in platform
+ double AngVelPlatform[3]; //!< r/s Angular velocity in platform frame
+ double timeTag; //!< [-] Observation time tag
+ double numberOfValidGyroMeasurements; //!< Number of valid gyro measurements
+} IMUSensorMsgPayload;
#endif
diff --git a/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.cpp b/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.cpp
index cf9666bbdf..8cc5701197 100644
--- a/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.cpp
+++ b/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.cpp
@@ -20,41 +20,41 @@
#include "inertialAttitudeUkf.h"
-InertialAttitudeUkf::InertialAttitudeUkf(AttitudeFilterMethod method){
- this->measurementAcceptanceMethod = method;
-}
+InertialAttitudeUkf::InertialAttitudeUkf(AttitudeFilterMethod method) { this->measurementAcceptanceMethod = method; }
-void InertialAttitudeUkf::customreset(){
+void InertialAttitudeUkf::customreset() {
/*! No custom reset for this module */
- std::function<FilterStateVector(double, const FilterStateVector)> attitudeDynamics = [this](double t, const FilterStateVector &state){
- Eigen::Vector3d mrp(state.getPositionStates());
- Eigen::Vector3d omega(state.getVelocityStates());
- Eigen::MatrixXd bMat = bmatMrp(mrp);
-
- FilterStateVector stateDerivative;
- PositionState mrpDot;
- mrpDot.setValues(0.25*bMat*omega);
- stateDerivative.setPosition(mrpDot);
-
- Eigen::Vector3d wheelTorque = Eigen::Vector3d::Zero();
- for(int i=0; i<this->rwArrayConfigPayload.numRW; i++){
- Eigen::Vector3d gsMatrix = Eigen::Map<Eigen::Vector3d>(&this->rwArrayConfigPayload.GsMatrix_B[i*3]);
- wheelTorque -= this->wheelAccelerations[i]*this->rwArrayConfigPayload.JsList[i]*gsMatrix;
- }
-
- VelocityState omegaDot;
- omegaDot.setValues(-this->spacecraftInertiaInverse*(tildeMatrix(omega)*this->spacecraftInertia*omega + wheelTorque));
- stateDerivative.setVelocity(omegaDot);
-
- return stateDerivative;
- };
+ std::function<FilterStateVector(double, const FilterStateVector)> attitudeDynamics =
+ [this](double t, const FilterStateVector &state) {
+ Eigen::Vector3d mrp(state.getPositionStates());
+ Eigen::Vector3d omega(state.getVelocityStates());
+ Eigen::MatrixXd bMat = bmatMrp(mrp);
+
+ FilterStateVector stateDerivative;
+ PositionState mrpDot;
+ mrpDot.setValues(0.25 * bMat * omega);
+ stateDerivative.setPosition(mrpDot);
+
+ Eigen::Vector3d wheelTorque = Eigen::Vector3d::Zero();
+ for (int i = 0; i < this->rwArrayConfigPayload.numRW; i++) {
+ Eigen::Vector3d gsMatrix = Eigen::Map<Eigen::Vector3d>(&this->rwArrayConfigPayload.GsMatrix_B[i * 3]);
+ wheelTorque -= this->wheelAccelerations[i] * this->rwArrayConfigPayload.JsList[i] * gsMatrix;
+ }
+
+ VelocityState omegaDot;
+ omegaDot.setValues(-this->spacecraftInertiaInverse *
+ (tildeMatrix(omega) * this->spacecraftInertia * omega + wheelTorque));
+ stateDerivative.setVelocity(omegaDot);
+
+ return stateDerivative;
+ };
this->dynamics.setDynamics(attitudeDynamics);
}
/*! Before every update, check the MRP norm for a shadow set switch
@return void
*/
-void InertialAttitudeUkf::customInitializeUpdate(){
+void InertialAttitudeUkf::customInitializeUpdate() {
PositionState mrp;
mrp.setValues(mrpSwitch(this->state.getPositionStates(), this->mrpSwitchThreshold));
this->state.setPosition(mrp);
@@ -63,9 +63,9 @@ void InertialAttitudeUkf::customInitializeUpdate(){
/*! After every update, check the MRP norm for a shadow set switch
@return void
*/
-void InertialAttitudeUkf::customFinalizeUpdate(){
+void InertialAttitudeUkf::customFinalizeUpdate() {
PositionState mrp;
- mrp.setValues( mrpSwitch(this->state.getPositionStates(), this->mrpSwitchThreshold));
+ mrp.setValues(mrpSwitch(this->state.getPositionStates(), this->mrpSwitchThreshold));
this->state.setPosition(mrp);
}
@@ -90,25 +90,25 @@ void InertialAttitudeUkf::writeOutputMessages(uint64_t currentSimNanos) {
eigenMatrixXd2CArray(this->xBar.returnValues(), filterPayload.stateError);
eigenMatrixXd2CArray(this->covar, filterPayload.covar);
- for (size_t index = 0; index < MAX_MEASUREMENT_NUMBER; index ++){
+ for (size_t index = 0; index < MAX_MEASUREMENT_NUMBER; index++) {
if (this->measurements[index].has_value()) {
auto measurement = this->measurements[index].value();
- if (measurement.getMeasurementName() == "starTracker"){
+ if (measurement.getMeasurementName() == "starTracker") {
starTrackerPayload.valid = true;
starTrackerPayload.numberOfObservations = 1;
starTrackerPayload.sizeOfObservations = measurement.size();
eigenMatrixXd2CArray(measurement.getObservation(), &starTrackerPayload.observation[0]);
eigenMatrixXd2CArray(measurement.getPostFitResiduals(), &starTrackerPayload.postFits[0]);
eigenMatrixXd2CArray(measurement.getPreFitResiduals(), &starTrackerPayload.preFits[0]);
- }
- if (measurement.getMeasurementName() == "gyro"){
+ }
+ if (measurement.getMeasurementName() == "gyro") {
gyroPayload.valid = true;
gyroPayload.numberOfObservations = 1;
gyroPayload.sizeOfObservations = measurement.size();
eigenMatrixXd2CArray(measurement.getObservation(), &gyroPayload.observation[0]);
eigenMatrixXd2CArray(measurement.getPostFitResiduals(), &gyroPayload.postFits[0]);
eigenMatrixXd2CArray(measurement.getPreFitResiduals(), &gyroPayload.preFits[0]);
- }
+ }
this->measurements[index].reset();
}
}
@@ -120,65 +120,68 @@ void InertialAttitudeUkf::writeOutputMessages(uint64_t currentSimNanos) {
}
/*! Read current RW speends and populate the accelerations in order to propagate
-* @return void
-* */
-void InertialAttitudeUkf::readRWSpeedData(){
+ * @return void
+ * */
+void InertialAttitudeUkf::readRWSpeedData() {
RWSpeedMsgPayload rwSpeedPayload = this->rwSpeedMsg();
uint64_t wheelSpeedTime = this->rwSpeedMsg.timeWritten();
- if (this->firstFilterPass){
- this->wheelAccelerations << 0,0,0,0;
+ if (this->firstFilterPass) {
+ this->wheelAccelerations << 0, 0, 0, 0;
this->previousWheelSpeeds = Eigen::Map<Eigen::Matrix<double, 1, 4>>(rwSpeedPayload.wheelSpeeds);
- this->previousWheelSpeedTime = wheelSpeedTime*NANO2SEC;
- }
- else{
- double dt = wheelSpeedTime*NANO2SEC - this->previousWheelSpeedTime;
- this->wheelAccelerations = (Eigen::Map<Eigen::Matrix<double, 1, 4>>(rwSpeedPayload.wheelSpeeds) - this->previousWheelSpeeds)/dt;
+ this->previousWheelSpeedTime = wheelSpeedTime * NANO2SEC;
+ } else {
+ double dt = wheelSpeedTime * NANO2SEC - this->previousWheelSpeedTime;
+ this->wheelAccelerations =
+ (Eigen::Map<Eigen::Matrix<double, 1, 4>>(rwSpeedPayload.wheelSpeeds) - this->previousWheelSpeeds) / dt;
}
}
/*! Loop through the all the input star trackers and populate their measurement container if they are foward
* in time
-* @return void
-* */
-void InertialAttitudeUkf::readStarTrackerData(){
- for (int index = 0; index < this->numberOfStarTackers; index ++){
+ * @return void
+ * */
+void InertialAttitudeUkf::readStarTrackerData() {
+ for (int index = 0; index < this->numberOfStarTackers; index++) {
auto starTracker = this->starTrackerMessages[index].starTrackerMsg();
- if (starTracker.timeTag*NANO2SEC > this->previousFilterTimeTag){
+ if (starTracker.timeTag * NANO2SEC > this->previousFilterTimeTag) {
auto starTrackerMeasurement = MeasurementModel();
starTrackerMeasurement.setMeasurementName("starTracker");
- starTrackerMeasurement.setTimeTag(starTracker.timeTag*NANO2SEC);
+ starTrackerMeasurement.setTimeTag(starTracker.timeTag * NANO2SEC);
starTrackerMeasurement.setValidity(true);
- starTrackerMeasurement.setMeasurementNoise(
- this->measNoiseScaling * this->starTrackerMessages[index].measurementNoise);
- starTrackerMeasurement.setObservation(mrpSwitch(Eigen::Map<Eigen::Vector3d>(starTracker.MRP_BdyInrtl),
- this->mrpSwitchThreshold));
+ starTrackerMeasurement.setMeasurementNoise(this->measNoiseScaling *
+ this->starTrackerMessages[index].measurementNoise);
+ starTrackerMeasurement.setObservation(
+ mrpSwitch(Eigen::Map<Eigen::Vector3d>(starTracker.MRP_BdyInrtl), this->mrpSwitchThreshold));
starTrackerMeasurement.setMeasurementModel(MeasurementModel::mrpStates);
this->measurements[this->measurementIndex] = starTrackerMeasurement;
this->measurementIndex += 1;
this->validStarTracker = true;
/*! - Only consider the filter started once a Star Tracker image is processed */
- if (this->firstFilterPass){this->firstFilterPass = false;}
+ if (this->firstFilterPass) {
+ this->firstFilterPass = false;
+ }
+ } else {
+ this->validStarTracker = false;
}
- else{this->validStarTracker=false;}
}
}
/*! Loop through the entire gyro buffer to find the first index that is in the future compared to the
-* previousFilterTimeTag. This does not assume the data comes in chronological order since the gyro data
-* is a ring buffer and can wrap around
-* @return void
-* */
-void InertialAttitudeUkf::readGyroData(){
+ * previousFilterTimeTag. This does not assume the data comes in chronological order since the gyro data
+ * is a ring buffer and can wrap around
+ * @return void
+ * */
+void InertialAttitudeUkf::readGyroData() {
IMUSensorMsgPayload gyroBuffer = this->imuSensorDataInMsg();
- if (gyroBuffer.timeTag*NANO2SEC > this->previousFilterTimeTag) {
+ if (gyroBuffer.timeTag * NANO2SEC > this->previousFilterTimeTag) {
auto gyroMeasurement = MeasurementModel();
gyroMeasurement.setMeasurementName("gyro");
gyroMeasurement.setTimeTag(gyroBuffer.timeTag);
gyroMeasurement.setValidity(true);
- gyroMeasurement.setMeasurementNoise(
- this->measNoiseScaling * this->gyroNoise/std::sqrt(gyroBuffer.numberOfValidGyroMeasurements));
+ gyroMeasurement.setMeasurementNoise(this->measNoiseScaling * this->gyroNoise /
+ std::sqrt(gyroBuffer.numberOfValidGyroMeasurements));
gyroMeasurement.setObservation(cArray2EigenVector3d(gyroBuffer.AngVelPlatform));
gyroMeasurement.setMeasurementModel(MeasurementModel::velocityStates);
this->measurements[this->measurementIndex] = gyroMeasurement;
@@ -218,21 +221,17 @@ void InertialAttitudeUkf::readFilterMeasurements() {
@param Eigen::Matrix3d gyroNoise
@return void
*/
-void InertialAttitudeUkf::setGyroNoise(const Eigen::Matrix3d &gyroNoiseInput) {
- this->gyroNoise = gyroNoiseInput;
-}
+void InertialAttitudeUkf::setGyroNoise(const Eigen::Matrix3d &gyroNoiseInput) { this->gyroNoise = gyroNoiseInput; }
/*! Get the gyro measurement noise matrix
@return Eigen::Matrix3d gyroNoise
*/
-Eigen::Matrix3d InertialAttitudeUkf::getGyroNoise() const {
- return this->gyroNoise;
-}
+Eigen::Matrix3d InertialAttitudeUkf::getGyroNoise() const { return this->gyroNoise; }
/*! Add a star tracker to the filter solution using the StarTrackerMessage class
@return StarTrackerMessage starTracker
*/
-void InertialAttitudeUkf::addStarTrackerInput(const StarTrackerMessage &starTracker){
+void InertialAttitudeUkf::addStarTrackerInput(const StarTrackerMessage &starTracker) {
this->starTrackerMessages[this->numberOfStarTackers] = starTracker;
this->numberOfStarTackers += 1;
}
@@ -250,7 +249,7 @@ Eigen::Matrix3d InertialAttitudeUkf::getStarTrackerNoise(int starTrackerNumber)
@param double step
@param double frequencyCutOff
*/
-void InertialAttitudeUkf::setLowPassFilter(double step, double frequencyCutOff){
+void InertialAttitudeUkf::setLowPassFilter(double step, double frequencyCutOff) {
this->hStep = step;
this->cutOffFrequency = frequencyCutOff;
}
diff --git a/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.h b/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.h
index 70b9219ff5..6de6c6d7f5 100644
--- a/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.h
+++ b/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.h
@@ -27,39 +27,36 @@
#include "architecture/_GeneralModuleFiles/sys_model.h"
#include "architecture/messaging/messaging.h"
-#include "architecture/utilities/macroDefinitions.h"
-#include "architecture/utilities/rigidBodyKinematics.hpp"
-#include "architecture/utilities/signalProcessing.h"
-
-#include "architecture/msgPayloadDefC/NavAttMsgPayload.h"
-#include "architecture/msgPayloadDefCpp/FilterMsgPayload.h"
-#include "architecture/msgPayloadDefCpp/FilterResidualsMsgPayload.h"
-
-#include "architecture/msgPayloadDefC/STAttMsgPayload.h"
#include "architecture/msgPayloadDefC/IMUSensorMsgPayload.h"
+#include "architecture/msgPayloadDefC/NavAttMsgPayload.h"
+#include "architecture/msgPayloadDefC/RWArrayConfigMsgPayload.h"
#include "architecture/msgPayloadDefC/RWSpeedMsgPayload.h"
+#include "architecture/msgPayloadDefC/STAttMsgPayload.h"
#include "architecture/msgPayloadDefC/VehicleConfigMsgPayload.h"
-#include "architecture/msgPayloadDefC/RWArrayConfigMsgPayload.h"
-
-#include "fswAlgorithms/_GeneralModuleFiles/srukfInterface.h"
+#include "architecture/msgPayloadDefCpp/FilterMsgPayload.h"
+#include "architecture/msgPayloadDefCpp/FilterResidualsMsgPayload.h"
+#include "architecture/utilities/macroDefinitions.h"
+#include "architecture/utilities/rigidBodyKinematics.hpp"
+#include "architecture/utilities/signalProcessing.h"
#include "fswAlgorithms/_GeneralModuleFiles/measurementModels.h"
+#include "fswAlgorithms/_GeneralModuleFiles/srukfInterface.h"
/*! @brief Star Tracker (ST) sensor container class. Contains the msg input name and Id and sensor noise value. */
-class StarTrackerMessage{
-public:
- ReadFunctor<STAttMsgPayload> starTrackerMsg; //!< star tracker input message
- Eigen::Matrix3d measurementNoise; //!< [-] Per axis noise on the ST
+class StarTrackerMessage {
+ public:
+ ReadFunctor<STAttMsgPayload> starTrackerMsg; //!< star tracker input message
+ Eigen::Matrix3d measurementNoise; //!< [-] Per axis noise on the ST
};
-enum class AttitudeFilterMethod {StarOnly, GyroWhenDazzled, AllMeasurements};
+enum class AttitudeFilterMethod { StarOnly, GyroWhenDazzled, AllMeasurements };
/*! @brief Inertial Attitude filter which reads Star Tracker measurements and gyro measurements */
-class InertialAttitudeUkf: public SRukfInterface {
-public:
+class InertialAttitudeUkf : public SRukfInterface {
+ public:
InertialAttitudeUkf(AttitudeFilterMethod method);
~InertialAttitudeUkf() = default;
-private:
+ private:
void customreset() final;
void readFilterMeasurements() final;
void writeOutputMessages(uint64_t currentSimNanos) final;
@@ -71,8 +68,7 @@ class InertialAttitudeUkf: public SRukfInterface {
void readStarTrackerData();
void readGyroData();
-
-public:
+ public:
ReadFunctor<RWArrayConfigMsgPayload> rwArrayConfigMsg;
RWArrayConfigMsgPayload rwArrayConfigPayload;
ReadFunctor<VehicleConfigMsgPayload> vehicleConfigMsg;
@@ -90,7 +86,7 @@ class InertialAttitudeUkf: public SRukfInterface {
Eigen::Matrix3d getStarTrackerNoise(int starTrackerNumber) const;
void setLowPassFilter(double step, double frequencyCutOff);
-private:
+ private:
AttitudeFilterMethod measurementAcceptanceMethod;
bool firstFilterPass = true;
bool validStarTracker = false;
@@ -101,14 +97,14 @@ class InertialAttitudeUkf: public SRukfInterface {
Eigen::Matrix3d spacecraftInertiaInverse;
double previousWheelSpeedTime = 0;
double hStep = 0.5;
- double cutOffFrequency = 15.0/(2*M_PI);
+ double cutOffFrequency = 15.0 / (2 * M_PI);
Eigen::Vector3d filteredOmega_BN_B;
Eigen::Matrix3d gyroNoise;
std::array<StarTrackerMessage, MAX_ST_VEH_COUNT> starTrackerMessages;
int numberOfStarTackers = 0;
int measurementIndex = 0;
- double mrpSwitchThreshold = 1; //!< [-] Threshold for switching MRP to/from the shadow set
+ double mrpSwitchThreshold = 1; //!< [-] Threshold for switching MRP to/from the shadow set
};
#endif
diff --git a/src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.cpp b/src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.cpp
old mode 100755
new mode 100644
index ec93596be2..d80486afeb
--- a/src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.cpp
+++ b/src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.cpp
@@ -1,4 +1,5 @@
#include "miruLowPassFilterConverter.h"
+
#include "architecture/utilities/avsEigenSupport.h"
/*! This method checks the input message to ensure it is linked.
@@ -23,8 +24,8 @@ void MiruLowPassFilterConverter::UpdateState(uint64_t callTime) {
double meanMeasurementTime = 0;
AccDataMsgPayload gyrBuffer = this->imuAccelDataInMsg();
for (int index = 0; index < MAX_ACC_BUF_PKT; index++) {
- double gyroMeasuredTime = gyrBuffer.accPkts[index].measTime*NANO2SEC;
- if (gyroMeasuredTime < firstFutureTime || firstFutureTime<0){
+ double gyroMeasuredTime = gyrBuffer.accPkts[index].measTime * NANO2SEC;
+ if (gyroMeasuredTime < firstFutureTime || firstFutureTime < 0) {
smallestFutureIndex = index;
firstFutureTime = gyroMeasuredTime;
}
@@ -33,7 +34,7 @@ void MiruLowPassFilterConverter::UpdateState(uint64_t callTime) {
}
lowPass.setFilterCutoff(this->cutOffFrequency);
lowPass.setFilterStep(this->hStep);
- if (numberOfValidGyroMeasurements > 0){
+ if (numberOfValidGyroMeasurements > 0) {
meanMeasurementTime /= numberOfValidGyroMeasurements;
/*! - Loop through buffer for all future measurements since the previous time to filter omega_BN_B*/
for (int index = 0; index < MAX_ACC_BUF_PKT; index++) {
diff --git a/src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.h b/src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.h
old mode 100755
new mode 100644
index 350973b919..2b0b2b73ca
--- a/src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.h
+++ b/src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.h
@@ -1,33 +1,34 @@
#ifndef _MIRULOWPASSFILTERCONVERTER_
#define _MIRULOWPASSFILTERCONVERTER_
+#include <Eigen/Core>
+
#include "architecture/_GeneralModuleFiles/sys_model.h"
#include "architecture/messaging/messaging.h"
-#include "architecture/utilities/bskLogging.h"
-#include "architecture/utilities/macroDefinitions.h"
#include "architecture/msgPayloadDefC/AccDataMsgPayload.h"
#include "architecture/msgPayloadDefC/IMUSensorMsgPayload.h"
+#include "architecture/utilities/bskLogging.h"
+#include "architecture/utilities/macroDefinitions.h"
#include "architecture/utilities/signalProcessing.h"
-#include <Eigen/Core>
/*! @brief Convert AccDataMsgPayload to IMUSensorMsgPayload Class */
-class MiruLowPassFilterConverter: public SysModel {
-public:
- MiruLowPassFilterConverter() = default; //!< Constructor
- ~MiruLowPassFilterConverter() = default; //!< Destructor
+class MiruLowPassFilterConverter : public SysModel {
+ public:
+ MiruLowPassFilterConverter() = default; //!< Constructor
+ ~MiruLowPassFilterConverter() = default; //!< Destructor
- void Reset(uint64_t CurrentSimNanos); //!< Reset member function
- void UpdateState(uint64_t CurrentSimNanos); //!< Update member function
- void setLowPassFilter(double step, double frequencyCutOff); //!< Setter method for the low pass filter
+ void Reset(uint64_t CurrentSimNanos); //!< Reset member function
+ void UpdateState(uint64_t CurrentSimNanos); //!< Update member function
+ void setLowPassFilter(double step, double frequencyCutOff); //!< Setter method for the low pass filter
- ReadFunctor<AccDataMsgPayload> imuAccelDataInMsg; //!< Input msg for the imu data
- Message<IMUSensorMsgPayload> imuSensorOutMsg; //!< Output msg for the imu data
+ ReadFunctor<AccDataMsgPayload> imuAccelDataInMsg; //!< Input msg for the imu data
+ Message<IMUSensorMsgPayload> imuSensorOutMsg; //!< Output msg for the imu data
- BSKLogger *bskLogger; //!< BSK Logging
+ BSKLogger *bskLogger; //!< BSK Logging
-private:
- double cutOffFrequency = 15.0/(2*M_PI); //!< Low pass filter parameter
- double hStep = 0.5; //!< Low pass filter parameter
+ private:
+ double cutOffFrequency = 15.0 / (2 * M_PI); //!< Low pass filter parameter
+ double hStep = 0.5; //!< Low pass filter parameter
};
#endif