Skip to content

Commit

Permalink
coarseSunSensor refactor: add a prop matrix setter + getter
Browse files Browse the repository at this point in the history
  • Loading branch information
Natsoulas committed Dec 23, 2024
1 parent 61b19f4 commit d7f2987
Show file tree
Hide file tree
Showing 2 changed files with 39 additions and 5 deletions.
30 changes: 30 additions & 0 deletions src/simulation/sensors/coarseSunSensor/coarseSunSensor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,8 @@ CoarseSunSensor::CoarseSunSensor()
this->sunVisibilityFactor.shadowFactor = 1.0;
this->sunDistanceFactor = 1.0;
this->dcm_PB.setIdentity(3,3);
this->propagationMatrix.resize(1);
this->propagationMatrix(0) = 1.0;
return;
}

Expand Down Expand Up @@ -170,6 +172,9 @@ void CoarseSunSensor::Reset(uint64_t CurrentSimNanos)
satBounds(0,1) = this->maxOutput;
this->saturateUtility.setBounds(satBounds);

// Set up noise model with stored propagation matrix
this->noiseModel.setPropMatrix(this->propagationMatrix);
this->faultNoiseModel.setPropMatrix(this->propagationMatrix);
}

void CoarseSunSensor::readInputMessages()
Expand Down Expand Up @@ -435,3 +440,28 @@ void CSSConstellation::appendCSS(CoarseSunSensor* newSensor) {
sensorList.push_back(newSensor);
return;
}

/*!
Setter for `AMatrix` used for error propagation
@param propMatrix Matrix to set
*/
void CoarseSunSensor::setAMatrix(const Eigen::Matrix<double, -1, 1, 0, -1, 1>& propMatrix)
{
if(propMatrix.rows() != 1 || propMatrix.cols() != 1) {
bskLogger.bskLog(BSK_ERROR, "CoarseSunSensor: Propagation matrix must be 1x1");
return;
}
this->propagationMatrix = propMatrix;
// Set the propagation matrix for both noise models
this->noiseModel.setPropMatrix(propMatrix);
this->faultNoiseModel.setPropMatrix(propMatrix);
}

/*!
Getter for `AMatrix` used for error propagation
@return Current matrix
*/
Eigen::Matrix<double, -1, 1, 0, -1, 1> CoarseSunSensor::getAMatrix() const
{
return this->propagationMatrix;
}
14 changes: 9 additions & 5 deletions src/simulation/sensors/coarseSunSensor/coarseSunSensor.h
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ class CoarseSunSensor: public SysModel {
void scaleSensorValues(); //!< scale the sensor values
void applySaturation(); //!< apply saturation effects to sensed output (floor and ceiling)
void writeOutputMessages(uint64_t Clock); //!< @brief method to write the output message to the system

public:
ReadFunctor<SpicePlanetStateMsgPayload> sunInMsg; //!< [-] input message for sun data
ReadFunctor<SCStatesMsgPayload> stateInMsg; //!< [-] input message for spacecraft state
Expand All @@ -88,7 +88,7 @@ class CoarseSunSensor: public SysModel {
double kellyFactor; //!< [-] Kelly curve fit for output cosine curve
double fov; //!< [-] rad, field of view half angle
Eigen::Vector3d r_B; //!< [m] position vector in body frame
Eigen::Vector3d r_PB_B; //!< [m] misalignment of CSS platform wrt spacecraft body frame
Eigen::Vector3d r_PB_B; //!< [m] misalignment of CSS platform wrt spacecraft body frame
double senBias; //!< [-] Sensor bias value
double senNoiseStd; //!< [-] Sensor noise value
double faultNoiseStd; //!< [-] Sensor noise value if CSSFAULT_RAND is triggered
Expand All @@ -99,6 +99,9 @@ class CoarseSunSensor: public SysModel {
int CSSGroupID=-1; //!< [-] (optional) CSS group id identifier, -1 means it is not set and default is used
BSKLogger bskLogger; //!< -- BSK Logging

void setAMatrix(const Eigen::Matrix<double, -1, 1, 0, -1, 1>& propMatrix);
Eigen::Matrix<double, -1, 1, 0, -1, 1> getAMatrix() const;

private:
SpicePlanetStateMsgPayload sunData; //!< [-] Unused for now, but including it for future
SCStatesMsgPayload stateCurrent; //!< [-] Current SSBI-relative state
Expand All @@ -107,11 +110,12 @@ class CoarseSunSensor: public SysModel {
GaussMarkov noiseModel; //! [-] Gauss Markov noise generation model
GaussMarkov faultNoiseModel; //! [-] Gauss Markov noise generation model exclusively for CSS fault
Saturate saturateUtility; //! [-] Saturation utility
Eigen::Matrix<double, -1, 1, 0, -1, 1> propagationMatrix; // Store the propagation matrix
};

//!@brief Constellation of coarse sun sensors for aggregating output information
/*! This class is a thin container on top of the above coarse-sun sensor class.
It is used to aggregate the output messages of the coarse sun-sensors into a
/*! This class is a thin container on top of the above coarse-sun sensor class.
It is used to aggregate the output messages of the coarse sun-sensors into a
a single output for use by downstream models.*/
class CSSConstellation: public SysModel {
public:
Expand All @@ -120,7 +124,7 @@ class CSSConstellation: public SysModel {
void Reset(uint64_t CurrentClock); //!< Method for reseting the module
void UpdateState(uint64_t CurrentSimNanos); //!< @brief [-] Main update method for CSS constellation
void appendCSS(CoarseSunSensor *newSensor); //!< @brief [-] Method for adding sensor to list

public:
Message<CSSArraySensorMsgPayload> constellationOutMsg; //!< [-] CSS constellation output message
std::vector<CoarseSunSensor *> sensorList; //!< [-] List of coarse sun sensors in constellation
Expand Down

0 comments on commit d7f2987

Please sign in to comment.