diff --git a/src/architecture/msgPayloadDefC/IMUSensorMsgPayload.h b/src/architecture/msgPayloadDefC/IMUSensorMsgPayload.h old mode 100755 new mode 100644 index fc0b1e19e9..6c5ef4c2c4 --- a/src/architecture/msgPayloadDefC/IMUSensorMsgPayload.h +++ b/src/architecture/msgPayloadDefC/IMUSensorMsgPayload.h @@ -20,14 +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 -}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 8f3467a353..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 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; irwArrayConfigPayload.numRW; i++){ - Eigen::Vector3d gsMatrix = Eigen::Map(&this->rwArrayConfigPayload.GsMatrix_B[i*3]); - wheelTorque -= this->wheelAccelerations[i]*this->rwArrayConfigPayload.JsList[i]*gsMatrix; - } + std::function 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(&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); + VelocityState omegaDot; + omegaDot.setValues(-this->spacecraftInertiaInverse * + (tildeMatrix(omega) * this->spacecraftInertia * omega + wheelTorque)); + stateDerivative.setVelocity(omegaDot); - return stateDerivative; - }; + 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,93 +120,69 @@ 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>(rwSpeedPayload.wheelSpeeds); - this->previousWheelSpeedTime = wheelSpeedTime*NANO2SEC; - } - else{ - double dt = wheelSpeedTime*NANO2SEC - this->previousWheelSpeedTime; - this->wheelAccelerations = (Eigen::Map>(rwSpeedPayload.wheelSpeeds) - this->previousWheelSpeeds)/dt; + this->previousWheelSpeedTime = wheelSpeedTime * NANO2SEC; + } else { + double dt = wheelSpeedTime * NANO2SEC - this->previousWheelSpeedTime; + this->wheelAccelerations = + (Eigen::Map>(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(starTracker.MRP_BdyInrtl), - this->mrpSwitchThreshold)); + starTrackerMeasurement.setMeasurementNoise(this->measNoiseScaling * + this->starTrackerMessages[index].measurementNoise); + starTrackerMeasurement.setObservation( + mrpSwitch(Eigen::Map(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(){ - 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(gyrBuffer.accPkts[shiftedIndex].gyro_B); - /*! - Apply low-pass filter to gyro measurements to get smoothed body rate*/ - lowPass.processMeasurement(omega_BN_B); - } - + * 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) { 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()); + gyroMeasurement.setMeasurementNoise(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; @@ -245,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 he gyro measurement noise matrix +/*! 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; } @@ -277,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 b2cebcf742..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/IMUSensorMsgPayload.h" #include "architecture/msgPayloadDefC/NavAttMsgPayload.h" -#include "architecture/msgPayloadDefCpp/FilterMsgPayload.h" -#include "architecture/msgPayloadDefCpp/FilterResidualsMsgPayload.h" - -#include "architecture/msgPayloadDefC/STAttMsgPayload.h" -#include "architecture/msgPayloadDefC/AccDataMsgPayload.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 starTrackerMsg; //!< star tracker input message - Eigen::Matrix3d measurementNoise; //!< [-] Per axis noise on the ST +class StarTrackerMessage { + public: + ReadFunctor 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,13 +68,12 @@ class InertialAttitudeUkf: public SRukfInterface { void readStarTrackerData(); void readGyroData(); - -public: + public: ReadFunctor rwArrayConfigMsg; RWArrayConfigMsgPayload rwArrayConfigPayload; ReadFunctor vehicleConfigMsg; ReadFunctor rwSpeedMsg; - ReadFunctor accelDataMsg; + ReadFunctor imuSensorDataInMsg; Message navAttitudeOutputMsg; Message inertialFilterOutputMsg; @@ -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 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/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; 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) diff --git a/src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.cpp b/src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.cpp new file mode 100644 index 0000000000..d80486afeb --- /dev/null +++ b/src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.cpp @@ -0,0 +1,64 @@ +#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(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 100644 index 0000000000..2b0b2b73ca --- /dev/null +++ b/src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.h @@ -0,0 +1,34 @@ +#ifndef _MIRULOWPASSFILTERCONVERTER_ +#define _MIRULOWPASSFILTERCONVERTER_ + +#include + +#include "architecture/_GeneralModuleFiles/sys_model.h" +#include "architecture/messaging/messaging.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" + +/*! @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 imuAccelDataInMsg; //!< Input msg for the imu data + Message 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__]) +%}