Skip to content

Commit 9fc0cac

Browse files
leahkinerleahkiner
committed
pre-commit changes
1 parent cd746a6 commit 9fc0cac

File tree

5 files changed

+121
-126
lines changed

5 files changed

+121
-126
lines changed

‎src/architecture/msgPayloadDefC/IMUSensorMsgPayload.h

100755100644
+8-10
Original file line numberDiff line numberDiff line change
@@ -20,16 +20,14 @@
2020
#ifndef _IMU_SENSOR_MESSAG_H
2121
#define _IMU_SENSOR_MESSAG_H
2222

23-
2423
//! @brief Simulated IMU Sensor output message definition.
25-
typedef struct{
26-
double DVFramePlatform[3]; //!< m/s Accumulated DVs in platform
27-
double AccelPlatform[3]; //!< m/s2 Apparent acceleration of the platform
28-
double DRFramePlatform[3]; //!< r Accumulated DRs in platform
29-
double AngVelPlatform[3]; //!< r/s Angular velocity in platform frame
30-
double timeTag; //!< [-] Observation time tag
31-
double numberOfValidGyroMeasurements; //!< Number of valid gyro measurements
32-
}IMUSensorMsgPayload;
33-
24+
typedef struct {
25+
double DVFramePlatform[3]; //!< m/s Accumulated DVs in platform
26+
double AccelPlatform[3]; //!< m/s2 Apparent acceleration of the platform
27+
double DRFramePlatform[3]; //!< r Accumulated DRs in platform
28+
double AngVelPlatform[3]; //!< r/s Angular velocity in platform frame
29+
double timeTag; //!< [-] Observation time tag
30+
double numberOfValidGyroMeasurements; //!< Number of valid gyro measurements
31+
} IMUSensorMsgPayload;
3432

3533
#endif

‎src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.cpp

+71-72
Original file line numberDiff line numberDiff line change
@@ -20,41 +20,41 @@
2020

2121
#include "inertialAttitudeUkf.h"
2222

23-
InertialAttitudeUkf::InertialAttitudeUkf(AttitudeFilterMethod method){
24-
this->measurementAcceptanceMethod = method;
25-
}
23+
InertialAttitudeUkf::InertialAttitudeUkf(AttitudeFilterMethod method) { this->measurementAcceptanceMethod = method; }
2624

27-
void InertialAttitudeUkf::customreset(){
25+
void InertialAttitudeUkf::customreset() {
2826
/*! No custom reset for this module */
29-
std::function<FilterStateVector(double, const FilterStateVector)> attitudeDynamics = [this](double t, const FilterStateVector &state){
30-
Eigen::Vector3d mrp(state.getPositionStates());
31-
Eigen::Vector3d omega(state.getVelocityStates());
32-
Eigen::MatrixXd bMat = bmatMrp(mrp);
33-
34-
FilterStateVector stateDerivative;
35-
PositionState mrpDot;
36-
mrpDot.setValues(0.25*bMat*omega);
37-
stateDerivative.setPosition(mrpDot);
38-
39-
Eigen::Vector3d wheelTorque = Eigen::Vector3d::Zero();
40-
for(int i=0; i<this->rwArrayConfigPayload.numRW; i++){
41-
Eigen::Vector3d gsMatrix = Eigen::Map<Eigen::Vector3d>(&this->rwArrayConfigPayload.GsMatrix_B[i*3]);
42-
wheelTorque -= this->wheelAccelerations[i]*this->rwArrayConfigPayload.JsList[i]*gsMatrix;
43-
}
44-
45-
VelocityState omegaDot;
46-
omegaDot.setValues(-this->spacecraftInertiaInverse*(tildeMatrix(omega)*this->spacecraftInertia*omega + wheelTorque));
47-
stateDerivative.setVelocity(omegaDot);
48-
49-
return stateDerivative;
50-
};
27+
std::function<FilterStateVector(double, const FilterStateVector)> attitudeDynamics =
28+
[this](double t, const FilterStateVector &state) {
29+
Eigen::Vector3d mrp(state.getPositionStates());
30+
Eigen::Vector3d omega(state.getVelocityStates());
31+
Eigen::MatrixXd bMat = bmatMrp(mrp);
32+
33+
FilterStateVector stateDerivative;
34+
PositionState mrpDot;
35+
mrpDot.setValues(0.25 * bMat * omega);
36+
stateDerivative.setPosition(mrpDot);
37+
38+
Eigen::Vector3d wheelTorque = Eigen::Vector3d::Zero();
39+
for (int i = 0; i < this->rwArrayConfigPayload.numRW; i++) {
40+
Eigen::Vector3d gsMatrix = Eigen::Map<Eigen::Vector3d>(&this->rwArrayConfigPayload.GsMatrix_B[i * 3]);
41+
wheelTorque -= this->wheelAccelerations[i] * this->rwArrayConfigPayload.JsList[i] * gsMatrix;
42+
}
43+
44+
VelocityState omegaDot;
45+
omegaDot.setValues(-this->spacecraftInertiaInverse *
46+
(tildeMatrix(omega) * this->spacecraftInertia * omega + wheelTorque));
47+
stateDerivative.setVelocity(omegaDot);
48+
49+
return stateDerivative;
50+
};
5151
this->dynamics.setDynamics(attitudeDynamics);
5252
}
5353

5454
/*! Before every update, check the MRP norm for a shadow set switch
5555
@return void
5656
*/
57-
void InertialAttitudeUkf::customInitializeUpdate(){
57+
void InertialAttitudeUkf::customInitializeUpdate() {
5858
PositionState mrp;
5959
mrp.setValues(mrpSwitch(this->state.getPositionStates(), this->mrpSwitchThreshold));
6060
this->state.setPosition(mrp);
@@ -63,9 +63,9 @@ void InertialAttitudeUkf::customInitializeUpdate(){
6363
/*! After every update, check the MRP norm for a shadow set switch
6464
@return void
6565
*/
66-
void InertialAttitudeUkf::customFinalizeUpdate(){
66+
void InertialAttitudeUkf::customFinalizeUpdate() {
6767
PositionState mrp;
68-
mrp.setValues( mrpSwitch(this->state.getPositionStates(), this->mrpSwitchThreshold));
68+
mrp.setValues(mrpSwitch(this->state.getPositionStates(), this->mrpSwitchThreshold));
6969
this->state.setPosition(mrp);
7070
}
7171

@@ -90,25 +90,25 @@ void InertialAttitudeUkf::writeOutputMessages(uint64_t currentSimNanos) {
9090
eigenMatrixXd2CArray(this->xBar.returnValues(), filterPayload.stateError);
9191
eigenMatrixXd2CArray(this->covar, filterPayload.covar);
9292

93-
for (size_t index = 0; index < MAX_MEASUREMENT_NUMBER; index ++){
93+
for (size_t index = 0; index < MAX_MEASUREMENT_NUMBER; index++) {
9494
if (this->measurements[index].has_value()) {
9595
auto measurement = this->measurements[index].value();
96-
if (measurement.getMeasurementName() == "starTracker"){
96+
if (measurement.getMeasurementName() == "starTracker") {
9797
starTrackerPayload.valid = true;
9898
starTrackerPayload.numberOfObservations = 1;
9999
starTrackerPayload.sizeOfObservations = measurement.size();
100100
eigenMatrixXd2CArray(measurement.getObservation(), &starTrackerPayload.observation[0]);
101101
eigenMatrixXd2CArray(measurement.getPostFitResiduals(), &starTrackerPayload.postFits[0]);
102102
eigenMatrixXd2CArray(measurement.getPreFitResiduals(), &starTrackerPayload.preFits[0]);
103-
}
104-
if (measurement.getMeasurementName() == "gyro"){
103+
}
104+
if (measurement.getMeasurementName() == "gyro") {
105105
gyroPayload.valid = true;
106106
gyroPayload.numberOfObservations = 1;
107107
gyroPayload.sizeOfObservations = measurement.size();
108108
eigenMatrixXd2CArray(measurement.getObservation(), &gyroPayload.observation[0]);
109109
eigenMatrixXd2CArray(measurement.getPostFitResiduals(), &gyroPayload.postFits[0]);
110110
eigenMatrixXd2CArray(measurement.getPreFitResiduals(), &gyroPayload.preFits[0]);
111-
}
111+
}
112112
this->measurements[index].reset();
113113
}
114114
}
@@ -120,65 +120,68 @@ void InertialAttitudeUkf::writeOutputMessages(uint64_t currentSimNanos) {
120120
}
121121

122122
/*! Read current RW speends and populate the accelerations in order to propagate
123-
* @return void
124-
* */
125-
void InertialAttitudeUkf::readRWSpeedData(){
123+
* @return void
124+
* */
125+
void InertialAttitudeUkf::readRWSpeedData() {
126126
RWSpeedMsgPayload rwSpeedPayload = this->rwSpeedMsg();
127127
uint64_t wheelSpeedTime = this->rwSpeedMsg.timeWritten();
128-
if (this->firstFilterPass){
129-
this->wheelAccelerations << 0,0,0,0;
128+
if (this->firstFilterPass) {
129+
this->wheelAccelerations << 0, 0, 0, 0;
130130
this->previousWheelSpeeds = Eigen::Map<Eigen::Matrix<double, 1, 4>>(rwSpeedPayload.wheelSpeeds);
131-
this->previousWheelSpeedTime = wheelSpeedTime*NANO2SEC;
132-
}
133-
else{
134-
double dt = wheelSpeedTime*NANO2SEC - this->previousWheelSpeedTime;
135-
this->wheelAccelerations = (Eigen::Map<Eigen::Matrix<double, 1, 4>>(rwSpeedPayload.wheelSpeeds) - this->previousWheelSpeeds)/dt;
131+
this->previousWheelSpeedTime = wheelSpeedTime * NANO2SEC;
132+
} else {
133+
double dt = wheelSpeedTime * NANO2SEC - this->previousWheelSpeedTime;
134+
this->wheelAccelerations =
135+
(Eigen::Map<Eigen::Matrix<double, 1, 4>>(rwSpeedPayload.wheelSpeeds) - this->previousWheelSpeeds) / dt;
136136
}
137137
}
138138

139139
/*! Loop through the all the input star trackers and populate their measurement container if they are foward
140140
* in time
141-
* @return void
142-
* */
143-
void InertialAttitudeUkf::readStarTrackerData(){
144-
for (int index = 0; index < this->numberOfStarTackers; index ++){
141+
* @return void
142+
* */
143+
void InertialAttitudeUkf::readStarTrackerData() {
144+
for (int index = 0; index < this->numberOfStarTackers; index++) {
145145
auto starTracker = this->starTrackerMessages[index].starTrackerMsg();
146-
if (starTracker.timeTag*NANO2SEC > this->previousFilterTimeTag){
146+
if (starTracker.timeTag * NANO2SEC > this->previousFilterTimeTag) {
147147
auto starTrackerMeasurement = MeasurementModel();
148148
starTrackerMeasurement.setMeasurementName("starTracker");
149-
starTrackerMeasurement.setTimeTag(starTracker.timeTag*NANO2SEC);
149+
starTrackerMeasurement.setTimeTag(starTracker.timeTag * NANO2SEC);
150150
starTrackerMeasurement.setValidity(true);
151151

152-
starTrackerMeasurement.setMeasurementNoise(
153-
this->measNoiseScaling * this->starTrackerMessages[index].measurementNoise);
154-
starTrackerMeasurement.setObservation(mrpSwitch(Eigen::Map<Eigen::Vector3d>(starTracker.MRP_BdyInrtl),
155-
this->mrpSwitchThreshold));
152+
starTrackerMeasurement.setMeasurementNoise(this->measNoiseScaling *
153+
this->starTrackerMessages[index].measurementNoise);
154+
starTrackerMeasurement.setObservation(
155+
mrpSwitch(Eigen::Map<Eigen::Vector3d>(starTracker.MRP_BdyInrtl), this->mrpSwitchThreshold));
156156
starTrackerMeasurement.setMeasurementModel(MeasurementModel::mrpStates);
157157
this->measurements[this->measurementIndex] = starTrackerMeasurement;
158158
this->measurementIndex += 1;
159159
this->validStarTracker = true;
160160
/*! - Only consider the filter started once a Star Tracker image is processed */
161-
if (this->firstFilterPass){this->firstFilterPass = false;}
161+
if (this->firstFilterPass) {
162+
this->firstFilterPass = false;
163+
}
164+
} else {
165+
this->validStarTracker = false;
162166
}
163-
else{this->validStarTracker=false;}
164167
}
165168
}
166169

167170
/*! Loop through the entire gyro buffer to find the first index that is in the future compared to the
168-
* previousFilterTimeTag. This does not assume the data comes in chronological order since the gyro data
169-
* is a ring buffer and can wrap around
170-
* @return void
171-
* */
172-
void InertialAttitudeUkf::readGyroData(){
171+
* previousFilterTimeTag. This does not assume the data comes in chronological order since the gyro data
172+
* is a ring buffer and can wrap around
173+
* @return void
174+
* */
175+
void InertialAttitudeUkf::readGyroData() {
173176
IMUSensorMsgPayload gyroBuffer = this->imuSensorDataInMsg();
174-
if (gyroBuffer.timeTag*NANO2SEC > this->previousFilterTimeTag) {
177+
if (gyroBuffer.timeTag * NANO2SEC > this->previousFilterTimeTag) {
175178
auto gyroMeasurement = MeasurementModel();
176179
gyroMeasurement.setMeasurementName("gyro");
177180
gyroMeasurement.setTimeTag(gyroBuffer.timeTag);
178181
gyroMeasurement.setValidity(true);
179182

180-
gyroMeasurement.setMeasurementNoise(
181-
this->measNoiseScaling * this->gyroNoise/std::sqrt(gyroBuffer.numberOfValidGyroMeasurements));
183+
gyroMeasurement.setMeasurementNoise(this->measNoiseScaling * this->gyroNoise /
184+
std::sqrt(gyroBuffer.numberOfValidGyroMeasurements));
182185
gyroMeasurement.setObservation(cArray2EigenVector3d(gyroBuffer.AngVelPlatform));
183186
gyroMeasurement.setMeasurementModel(MeasurementModel::velocityStates);
184187
this->measurements[this->measurementIndex] = gyroMeasurement;
@@ -218,21 +221,17 @@ void InertialAttitudeUkf::readFilterMeasurements() {
218221
@param Eigen::Matrix3d gyroNoise
219222
@return void
220223
*/
221-
void InertialAttitudeUkf::setGyroNoise(const Eigen::Matrix3d &gyroNoiseInput) {
222-
this->gyroNoise = gyroNoiseInput;
223-
}
224+
void InertialAttitudeUkf::setGyroNoise(const Eigen::Matrix3d &gyroNoiseInput) { this->gyroNoise = gyroNoiseInput; }
224225

225226
/*! Get the gyro measurement noise matrix
226227
@return Eigen::Matrix3d gyroNoise
227228
*/
228-
Eigen::Matrix3d InertialAttitudeUkf::getGyroNoise() const {
229-
return this->gyroNoise;
230-
}
229+
Eigen::Matrix3d InertialAttitudeUkf::getGyroNoise() const { return this->gyroNoise; }
231230

232231
/*! Add a star tracker to the filter solution using the StarTrackerMessage class
233232
@return StarTrackerMessage starTracker
234233
*/
235-
void InertialAttitudeUkf::addStarTrackerInput(const StarTrackerMessage &starTracker){
234+
void InertialAttitudeUkf::addStarTrackerInput(const StarTrackerMessage &starTracker) {
236235
this->starTrackerMessages[this->numberOfStarTackers] = starTracker;
237236
this->numberOfStarTackers += 1;
238237
}
@@ -250,7 +249,7 @@ Eigen::Matrix3d InertialAttitudeUkf::getStarTrackerNoise(int starTrackerNumber)
250249
@param double step
251250
@param double frequencyCutOff
252251
*/
253-
void InertialAttitudeUkf::setLowPassFilter(double step, double frequencyCutOff){
252+
void InertialAttitudeUkf::setLowPassFilter(double step, double frequencyCutOff) {
254253
this->hStep = step;
255254
this->cutOffFrequency = frequencyCutOff;
256255
}

‎src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.h

+21-25
Original file line numberDiff line numberDiff line change
@@ -27,39 +27,36 @@
2727

2828
#include "architecture/_GeneralModuleFiles/sys_model.h"
2929
#include "architecture/messaging/messaging.h"
30-
#include "architecture/utilities/macroDefinitions.h"
31-
#include "architecture/utilities/rigidBodyKinematics.hpp"
32-
#include "architecture/utilities/signalProcessing.h"
33-
34-
#include "architecture/msgPayloadDefC/NavAttMsgPayload.h"
35-
#include "architecture/msgPayloadDefCpp/FilterMsgPayload.h"
36-
#include "architecture/msgPayloadDefCpp/FilterResidualsMsgPayload.h"
37-
38-
#include "architecture/msgPayloadDefC/STAttMsgPayload.h"
3930
#include "architecture/msgPayloadDefC/IMUSensorMsgPayload.h"
31+
#include "architecture/msgPayloadDefC/NavAttMsgPayload.h"
32+
#include "architecture/msgPayloadDefC/RWArrayConfigMsgPayload.h"
4033
#include "architecture/msgPayloadDefC/RWSpeedMsgPayload.h"
34+
#include "architecture/msgPayloadDefC/STAttMsgPayload.h"
4135
#include "architecture/msgPayloadDefC/VehicleConfigMsgPayload.h"
42-
#include "architecture/msgPayloadDefC/RWArrayConfigMsgPayload.h"
43-
44-
#include "fswAlgorithms/_GeneralModuleFiles/srukfInterface.h"
36+
#include "architecture/msgPayloadDefCpp/FilterMsgPayload.h"
37+
#include "architecture/msgPayloadDefCpp/FilterResidualsMsgPayload.h"
38+
#include "architecture/utilities/macroDefinitions.h"
39+
#include "architecture/utilities/rigidBodyKinematics.hpp"
40+
#include "architecture/utilities/signalProcessing.h"
4541
#include "fswAlgorithms/_GeneralModuleFiles/measurementModels.h"
42+
#include "fswAlgorithms/_GeneralModuleFiles/srukfInterface.h"
4643

4744
/*! @brief Star Tracker (ST) sensor container class. Contains the msg input name and Id and sensor noise value. */
48-
class StarTrackerMessage{
49-
public:
50-
ReadFunctor<STAttMsgPayload> starTrackerMsg; //!< star tracker input message
51-
Eigen::Matrix3d measurementNoise; //!< [-] Per axis noise on the ST
45+
class StarTrackerMessage {
46+
public:
47+
ReadFunctor<STAttMsgPayload> starTrackerMsg; //!< star tracker input message
48+
Eigen::Matrix3d measurementNoise; //!< [-] Per axis noise on the ST
5249
};
5350

54-
enum class AttitudeFilterMethod {StarOnly, GyroWhenDazzled, AllMeasurements};
51+
enum class AttitudeFilterMethod { StarOnly, GyroWhenDazzled, AllMeasurements };
5552

5653
/*! @brief Inertial Attitude filter which reads Star Tracker measurements and gyro measurements */
57-
class InertialAttitudeUkf: public SRukfInterface {
58-
public:
54+
class InertialAttitudeUkf : public SRukfInterface {
55+
public:
5956
InertialAttitudeUkf(AttitudeFilterMethod method);
6057
~InertialAttitudeUkf() = default;
6158

62-
private:
59+
private:
6360
void customreset() final;
6461
void readFilterMeasurements() final;
6562
void writeOutputMessages(uint64_t currentSimNanos) final;
@@ -71,8 +68,7 @@ class InertialAttitudeUkf: public SRukfInterface {
7168
void readStarTrackerData();
7269
void readGyroData();
7370

74-
75-
public:
71+
public:
7672
ReadFunctor<RWArrayConfigMsgPayload> rwArrayConfigMsg;
7773
RWArrayConfigMsgPayload rwArrayConfigPayload;
7874
ReadFunctor<VehicleConfigMsgPayload> vehicleConfigMsg;
@@ -90,7 +86,7 @@ class InertialAttitudeUkf: public SRukfInterface {
9086
Eigen::Matrix3d getStarTrackerNoise(int starTrackerNumber) const;
9187
void setLowPassFilter(double step, double frequencyCutOff);
9288

93-
private:
89+
private:
9490
AttitudeFilterMethod measurementAcceptanceMethod;
9591
bool firstFilterPass = true;
9692
bool validStarTracker = false;
@@ -101,14 +97,14 @@ class InertialAttitudeUkf: public SRukfInterface {
10197
Eigen::Matrix3d spacecraftInertiaInverse;
10298
double previousWheelSpeedTime = 0;
10399
double hStep = 0.5;
104-
double cutOffFrequency = 15.0/(2*M_PI);
100+
double cutOffFrequency = 15.0 / (2 * M_PI);
105101

106102
Eigen::Vector3d filteredOmega_BN_B;
107103
Eigen::Matrix3d gyroNoise;
108104
std::array<StarTrackerMessage, MAX_ST_VEH_COUNT> starTrackerMessages;
109105
int numberOfStarTackers = 0;
110106
int measurementIndex = 0;
111-
double mrpSwitchThreshold = 1; //!< [-] Threshold for switching MRP to/from the shadow set
107+
double mrpSwitchThreshold = 1; //!< [-] Threshold for switching MRP to/from the shadow set
112108
};
113109

114110
#endif

‎src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.cpp

100755100644
+4-3
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
#include "miruLowPassFilterConverter.h"
2+
23
#include "architecture/utilities/avsEigenSupport.h"
34

45
/*! This method checks the input message to ensure it is linked.
@@ -23,8 +24,8 @@ void MiruLowPassFilterConverter::UpdateState(uint64_t callTime) {
2324
double meanMeasurementTime = 0;
2425
AccDataMsgPayload gyrBuffer = this->imuAccelDataInMsg();
2526
for (int index = 0; index < MAX_ACC_BUF_PKT; index++) {
26-
double gyroMeasuredTime = gyrBuffer.accPkts[index].measTime*NANO2SEC;
27-
if (gyroMeasuredTime < firstFutureTime || firstFutureTime<0){
27+
double gyroMeasuredTime = gyrBuffer.accPkts[index].measTime * NANO2SEC;
28+
if (gyroMeasuredTime < firstFutureTime || firstFutureTime < 0) {
2829
smallestFutureIndex = index;
2930
firstFutureTime = gyroMeasuredTime;
3031
}
@@ -33,7 +34,7 @@ void MiruLowPassFilterConverter::UpdateState(uint64_t callTime) {
3334
}
3435
lowPass.setFilterCutoff(this->cutOffFrequency);
3536
lowPass.setFilterStep(this->hStep);
36-
if (numberOfValidGyroMeasurements > 0){
37+
if (numberOfValidGyroMeasurements > 0) {
3738
meanMeasurementTime /= numberOfValidGyroMeasurements;
3839
/*! - Loop through buffer for all future measurements since the previous time to filter omega_BN_B*/
3940
for (int index = 0; index < MAX_ACC_BUF_PKT; index++) {

0 commit comments

Comments
 (0)