Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Update inertial attitude ukf to accept IMUSensorMsgPayload #349

Open
wants to merge 5 commits into
base: develop
Choose a base branch
from
Open

Update inertial attitude ukf to accept IMUSensorMsgPayload #349

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
db5616c
Add timeTag and numberOfValidGyroMeasurements to IMUSensorMsgPayload
leahkiner Sep 27, 2024
8fe2fd8
Update inertial attitude ukf to accept IMUSensorMsgPayload
leahkiner Sep 27, 2024
0bc9f84
Add miruLowPassFilterConverter to sensorInterfaces
leahkiner Sep 30, 2024
cd746a6
Update inertialAttitudeUkf unit test
leahkiner Sep 30, 2024
9fc0cac
pre-commit changes
leahkiner Mar 3, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 8 additions & 8 deletions src/architecture/msgPayloadDefC/IMUSensorMsgPayload.h
100755 → 100644
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
174 changes: 73 additions & 101 deletions src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
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);
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);
Expand All @@ -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);
}

Expand All @@ -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();
}
}
Expand All @@ -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<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(){
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);
}

* 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;
Expand Down Expand Up @@ -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;
}
Expand All @@ -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;
}
48 changes: 22 additions & 26 deletions src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<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;
Expand All @@ -71,13 +68,12 @@ class InertialAttitudeUkf: public SRukfInterface {
void readStarTrackerData();
void readGyroData();


public:
public:
ReadFunctor<RWArrayConfigMsgPayload> rwArrayConfigMsg;
RWArrayConfigMsgPayload rwArrayConfigPayload;
ReadFunctor<VehicleConfigMsgPayload> vehicleConfigMsg;
ReadFunctor<RWSpeedMsgPayload> rwSpeedMsg;
ReadFunctor<AccDataMsgPayload> accelDataMsg;
ReadFunctor<IMUSensorMsgPayload> imuSensorDataInMsg;

Message<NavAttMsgPayload> navAttitudeOutputMsg;
Message<FilterMsgPayload> inertialFilterOutputMsg;
Expand All @@ -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;
Expand All @@ -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
Loading