diff --git a/src/simulation/sensors/coarseSunSensor/coarseSunSensor.cpp b/src/simulation/sensors/coarseSunSensor/coarseSunSensor.cpp index 6f589d78cf..066bf23f00 100755 --- a/src/simulation/sensors/coarseSunSensor/coarseSunSensor.cpp +++ b/src/simulation/sensors/coarseSunSensor/coarseSunSensor.cpp @@ -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; } @@ -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() @@ -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& 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 CoarseSunSensor::getAMatrix() const +{ + return this->propagationMatrix; +} diff --git a/src/simulation/sensors/coarseSunSensor/coarseSunSensor.h b/src/simulation/sensors/coarseSunSensor/coarseSunSensor.h index fe595a554d..dc62c6160d 100755 --- a/src/simulation/sensors/coarseSunSensor/coarseSunSensor.h +++ b/src/simulation/sensors/coarseSunSensor/coarseSunSensor.h @@ -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 sunInMsg; //!< [-] input message for sun data ReadFunctor stateInMsg; //!< [-] input message for spacecraft state @@ -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 @@ -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& propMatrix); + Eigen::Matrix getAMatrix() const; + private: SpicePlanetStateMsgPayload sunData; //!< [-] Unused for now, but including it for future SCStatesMsgPayload stateCurrent; //!< [-] Current SSBI-relative state @@ -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 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: @@ -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 constellationOutMsg; //!< [-] CSS constellation output message std::vector sensorList; //!< [-] List of coarse sun sensors in constellation