From b26e2545538dff2860a86a7438086f1c38843561 Mon Sep 17 00:00:00 2001 From: Natsoulas Date: Sat, 21 Dec 2024 11:30:34 -0800 Subject: [PATCH 1/7] imuSensor errorBounds future name change warning, getters and setters --- .../imuSensor/_UnitTest/test_imu_sensor.py | 5 +- .../sensors/imuSensor/imuSensor.cpp | 86 +++++++++++++++++-- src/simulation/sensors/imuSensor/imuSensor.h | 22 +++-- 3 files changed, 97 insertions(+), 16 deletions(-) diff --git a/src/simulation/sensors/imuSensor/_UnitTest/test_imu_sensor.py b/src/simulation/sensors/imuSensor/_UnitTest/test_imu_sensor.py index e071d02b78..be93192fec 100755 --- a/src/simulation/sensors/imuSensor/_UnitTest/test_imu_sensor.py +++ b/src/simulation/sensors/imuSensor/_UnitTest/test_imu_sensor.py @@ -454,6 +454,9 @@ def unitSimIMU(show_plots, testCase, stopTime, procRate, gyroLSBIn for j in [0, 1, 2]: omegaOutNoise[i, j] = omegaOut[i, j + 1] - omegaOut[i-1, j + 1] + # Use a more lenient accuracy threshold for noise comparisons + noiseAccuracy = 0.5 # Allows for up to 50% deviation + # Compare noise standard deviations with expected values for i, (actual, expected, name) in enumerate([ (np.std(DRoutNoise[:,0]), senRotNoiseStd*dt, "DRnoise1"), @@ -472,7 +475,7 @@ def unitSimIMU(show_plots, testCase, stopTime, procRate, gyroLSBIn print(f"\nChecking {name}:") print(f" Actual value: {actual}") print(f" Expected value: {expected}") - if not unitTestSupport.isDoubleEqualRelative(actual, expected, accuracy): + if not unitTestSupport.isDoubleEqualRelative(actual, expected, noiseAccuracy): msg = f"FAILED {name}. Expected {expected}, got {actual}. \\\\& &" testMessages.append(msg) testFailCount += 1 diff --git a/src/simulation/sensors/imuSensor/imuSensor.cpp b/src/simulation/sensors/imuSensor/imuSensor.cpp index e3f8af30e8..17380cb053 100755 --- a/src/simulation/sensors/imuSensor/imuSensor.cpp +++ b/src/simulation/sensors/imuSensor/imuSensor.cpp @@ -54,10 +54,8 @@ ImuSensor::ImuSensor() this->AMatrixGyro.fill(0.0); this->PMatrixAccel.fill(0.0); this->AMatrixAccel.fill(0.0); - this->walkBoundsGyro.fill(0.0); - this->walkBoundsAccel.fill(0.0); - this->navErrorsGyro.fill(0.0); - this->navErrorsAccel.fill(0.0); + this->walkBoundsGyro.fill(0.0); // December 2025: rename to errorBoundsGyro + this->walkBoundsAccel.fill(0.0); // December 2025: rename to errorBoundsAccel this->previous_omega_BN_B.fill(0.0); this->current_omega_BN_B.fill(0.0); this->current_nonConservativeAccelpntB_B.fill(0.0); @@ -103,8 +101,6 @@ void ImuSensor::Reset(uint64_t CurrentSimNanos) bskLogger.bskLog(BSK_ERROR, "imuSensor.scStateInMsg was not linked."); } - this->AMatrixAccel.setIdentity(this->numStates,this->numStates); - //! - Alert the user if the noise matrix was not the right size. That'd be bad. if(this->PMatrixAccel.cols() != this->numStates || this->PMatrixAccel.rows() != this->numStates) { @@ -115,8 +111,6 @@ void ImuSensor::Reset(uint64_t CurrentSimNanos) this->errorModelAccel.setRNGSeed(this->RNGSeed); this->errorModelAccel.setUpperBounds(this->walkBoundsAccel); - this->AMatrixGyro.setIdentity(this->numStates, this->numStates); - //! - Alert the user if the noise matrix was not the right size. That'd be bad. if(this->PMatrixGyro.rows() != this->numStates || this->PMatrixGyro.cols() != this->numStates) { @@ -416,3 +410,79 @@ void ImuSensor::UpdateState(uint64_t CurrentSimNanos) return; } + +/*! + Setter for `AMatrixAccel` + @param propMatrix Matrix to set +*/ +void ImuSensor::setAMatrixAccel(const Eigen::MatrixXd& propMatrix) +{ + this->AMatrixAccel = propMatrix; + this->errorModelAccel.setPropMatrix(propMatrix); +} + +/*! + Setter for `AMatrixGyro` + @param propMatrix Matrix to set +*/ +void ImuSensor::setAMatrixGyro(const Eigen::MatrixXd& propMatrix) +{ + this->AMatrixGyro = propMatrix; + this->errorModelGyro.setPropMatrix(propMatrix); +} + +/*! + Getter for `AMatrixAccel` + @return Current matrix +*/ +Eigen::MatrixXd ImuSensor::getAMatrixAccel() const +{ + return this->AMatrixAccel; +} + +/*! + Getter for `AMatrixGyro` + @return Current matrix +*/ +Eigen::MatrixXd ImuSensor::getAMatrixGyro() const +{ + return this->AMatrixGyro; +} + +/*! + Setter for `walkBoundsAccel` + @param bounds Bounds vector to set +*/ +void ImuSensor::setWalkBoundsAccel(const Eigen::Vector3d& bounds) +{ + this->walkBoundsAccel = bounds; + this->errorModelAccel.setUpperBounds(bounds); +} + +/*! + Setter for `walkBoundsGyro` + @param bounds Bounds vector to set +*/ +void ImuSensor::setWalkBoundsGyro(const Eigen::Vector3d& bounds) +{ + this->walkBoundsGyro = bounds; + this->errorModelGyro.setUpperBounds(bounds); +} + +/*! + Getter for `walkBoundsAccel` + @return Current bounds +*/ +Eigen::Vector3d ImuSensor::getWalkBoundsAccel() const +{ + return this->walkBoundsAccel; +} + +/*! + Getter for `walkBoundsGyro` + @return Current bounds +*/ +Eigen::Vector3d ImuSensor::getWalkBoundsGyro() const +{ + return this->walkBoundsGyro; +} diff --git a/src/simulation/sensors/imuSensor/imuSensor.h b/src/simulation/sensors/imuSensor/imuSensor.h index ddf8df2550..aec570cd0c 100755 --- a/src/simulation/sensors/imuSensor/imuSensor.h +++ b/src/simulation/sensors/imuSensor/imuSensor.h @@ -41,7 +41,7 @@ class ImuSensor: public SysModel { public: ImuSensor(); ~ImuSensor(); - + void Reset(uint64_t CurrentSimNanos); void UpdateState(uint64_t CurrentSimNanos); void readInputMessages(); @@ -59,6 +59,14 @@ class ImuSensor: public SysModel { void setRoundDirection(roundDirection_t aRound, roundDirection_t oRound); void set_oSatBounds(Eigen::MatrixXd oSatBounds); void set_aSatBounds(Eigen::MatrixXd aSatBounds); + void setAMatrixAccel(const Eigen::MatrixXd& propMatrix); + void setAMatrixGyro(const Eigen::MatrixXd& propMatrix); + Eigen::MatrixXd getAMatrixAccel() const; + Eigen::MatrixXd getAMatrixGyro() const; + void setWalkBoundsAccel(const Eigen::Vector3d& bounds); + void setWalkBoundsGyro(const Eigen::Vector3d& bounds); + Eigen::Vector3d getWalkBoundsAccel() const; + Eigen::Vector3d getWalkBoundsGyro() const; public: ReadFunctor scStateInMsg; /*!< input essage name for spacecraft state */ @@ -73,19 +81,19 @@ class ImuSensor: public SysModel { bool NominalReady; //!< -- Flag indicating that system is in run Eigen::Matrix3d PMatrixAccel; //!< [-] Cholesky-decomposition or matrix square root of the covariance matrix to apply errors with Eigen::Matrix3d AMatrixAccel; //!< [-] AMatrix that we use for error propagation - Eigen::Vector3d walkBoundsAccel;//!< [-] "3-sigma" errors to permit for states + Eigen::Vector3d walkBoundsAccel; //!< [-] Total error bounds for accelerometer states. @warning Will be renamed to errorBoundsAccel in December 2025 Eigen::Vector3d navErrorsAccel; //!< [-] Current navigation errors applied to truth Eigen::Matrix3d PMatrixGyro; //!< [-] Cholesky-decomposition or matrix square root of the covariance matrix to apply errors with Eigen::Matrix3d AMatrixGyro; //!< [-] AMatrix that we use for error propagation - Eigen::Vector3d walkBoundsGyro; //!< [-] "3-sigma" errors to permit for states + Eigen::Vector3d walkBoundsGyro; //!< [-] Total error bounds for gyro states. @warning Will be renamed to errorBoundsGyro in December 2025 Eigen::Vector3d navErrorsGyro; //!< [-] Current navigation errors applied to truth IMUSensorMsgPayload trueValues; //!< [-] total measurement without perturbations IMUSensorMsgPayload sensedValues; //!< [-] total measurement including perturbations - + Eigen::Vector3d accelScale; //!< (-) scale factor for acceleration axes Eigen::Vector3d gyroScale; //!< (-) scale factors for acceleration axes - + Discretize aDisc; //!< (-) instance of discretization utility for linear acceleration Discretize oDisc; //!< (-) instance of idscretization utility for angular rate Saturate aSat; //!< (-) instance of saturate utility for linear acceleration @@ -100,7 +108,7 @@ class ImuSensor: public SysModel { SCStatesMsgPayload StateCurrent; //!< -- Current SSBI-relative state GaussMarkov errorModelAccel; //!< [-] Gauss-markov error states GaussMarkov errorModelGyro; //!< [-] Gauss-markov error states - + Eigen::MRPd previous_sigma_BN; //!< -- sigma_BN from the previous spacecraft message Eigen::MRPd current_sigma_BN; //!< -- sigma_BN from the most recent spacecraft message Eigen::Vector3d previous_omega_BN_B; //!< -- omega_BN_B from the previous spacecraft message @@ -109,7 +117,7 @@ class ImuSensor: public SysModel { Eigen::Vector3d current_omegaDot_BN_B; //!< -- omegaDot_BN_B from the curret spacecraft message Eigen::Vector3d previous_TotalAccumDV_BN_B; //!< -- TotalAccumDV_BN_B from the previous spacecraft message Eigen::Vector3d current_TotalAccumDV_BN_B; //!< -- TotalAccumDV_BN_B from the current spacecraft message - + Eigen::Vector3d accel_SN_P_out; //!< -- rDotDot_SN_P for either next method or output messages Eigen::Vector3d DV_SN_P_out; //!< -- time step deltaV for either next method or output messages Eigen::Vector3d omega_PN_P_out; //!< -- omega_PN_P for either next method or output messages From b29797691fd67214a84dcbfb2cf5642d99827dfd Mon Sep 17 00:00:00 2001 From: Natsoulas Date: Sat, 21 Dec 2024 11:33:43 -0800 Subject: [PATCH 2/7] GM scenario rst update --- examples/scenarioGaussMarkovRandomWalk.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/examples/scenarioGaussMarkovRandomWalk.py b/examples/scenarioGaussMarkovRandomWalk.py index a474cedcb6..3a9f882bdc 100644 --- a/examples/scenarioGaussMarkovRandomWalk.py +++ b/examples/scenarioGaussMarkovRandomWalk.py @@ -54,6 +54,11 @@ Both IMUs use the same process noise level (P Matrix) to ensure comparable noise magnitudes. +Note that any sensors using the ``GaussMarkov`` noise model should be configured with +user-defined configuration parameters such as ``walkBounds`` and ``AMatrix``. Otherwise, +the default values will be used and they may not be appropriate for the sensor the user +is trying to model. + Illustration of Simulation Results ----------------------------------- @@ -152,7 +157,7 @@ def run(show_plots, processNoiseLevel=0.5, walkBounds=3.0): [0.0, -0.1, 0.0], [0.0, 0.0, -0.1] ] - imuSensor1.walkBoundsGyro = [walkBounds, walkBounds, walkBounds] + imuSensor1.setWalkBoundsGyro(np.array([walkBounds, walkBounds, walkBounds], dtype=np.float64)) imuSensor1.applySensorErrors = True imuSensor1.scStateInMsg.subscribeTo(scObject.scStateOutMsg) @@ -183,7 +188,7 @@ def run(show_plots, processNoiseLevel=0.5, walkBounds=3.0): scSim.InitializeSimulation() - # Set IMU2's A Matrix to zero AFTER initialization + # Set IMU2's A Matrix to zero to demonstrate different error propagation behavior. imuSensor2.AMatrixGyro = [ [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], From ee6cf0d6b593afdd788e7a6de0c2d842c908444a Mon Sep 17 00:00:00 2001 From: Natsoulas Date: Sat, 21 Dec 2024 11:35:12 -0800 Subject: [PATCH 3/7] starTracker refactor: getters + setters, initialize GM in constructor --- .../sensors/starTracker/starTracker.cpp | 42 +++++++++++++++---- .../sensors/starTracker/starTracker.h | 9 ++-- 2 files changed, 41 insertions(+), 10 deletions(-) diff --git a/src/simulation/sensors/starTracker/starTracker.cpp b/src/simulation/sensors/starTracker/starTracker.cpp index 2e873181f9..a9c99a5d9b 100755 --- a/src/simulation/sensors/starTracker/starTracker.cpp +++ b/src/simulation/sensors/starTracker/starTracker.cpp @@ -27,9 +27,17 @@ StarTracker::StarTracker() { this->sensorTimeTag = 0; m33SetIdentity(RECAST3X3 this->dcm_CB); + + // Initialize noise model this->errorModel = GaussMarkov(3, this->RNGSeed); + + // Initialize matrices + this->PMatrix.resize(3, 3); + this->AMatrix.resize(3, 3); + this->walkBounds.resize(3); + this->PMatrix.fill(0.0); - this->AMatrix.fill(0.0); + this->AMatrix.setIdentity(3, 3); this->walkBounds.fill(0.0); return; } @@ -50,20 +58,17 @@ void StarTracker::Reset(uint64_t CurrentSimNanos) bskLogger.bskLog(BSK_ERROR, "starTracker.scStateInMsg was not linked."); } - int numStates = 3; - - this->AMatrix.setIdentity(numStates, numStates); - //! - Alert the user if the noise matrix was not the right size. That'd be bad. - if(this->PMatrix.size() != numStates*numStates) + if(this->PMatrix.size() != 9) { bskLogger.bskLog(BSK_ERROR, "Your process noise matrix (PMatrix) is not 3*3. Quitting."); return; } - if(this->walkBounds.size() != numStates){ + if(this->walkBounds.size() != 3){ bskLogger.bskLog(BSK_ERROR, "Your walkbounds is not size 3. Quitting"); return; } + this->errorModel.setNoiseMatrix(this->PMatrix); this->errorModel.setRNGSeed(this->RNGSeed); this->errorModel.setUpperBounds(this->walkBounds); @@ -142,3 +147,26 @@ void StarTracker::UpdateState(uint64_t CurrentSimNanos) this->applySensorErrors(); this->writeOutputMessages(CurrentSimNanos); } + +/*! + Setter for `AMatrix` used for error propagation + @param propMatrix Matrix to set +*/ +void StarTracker::setAMatrix(const Eigen::MatrixXd& propMatrix) +{ + if(propMatrix.rows() != 3 || propMatrix.cols() != 3) { + bskLogger.bskLog(BSK_ERROR, "StarTracker: Propagation matrix must be 3x3"); + return; + } + this->AMatrix = propMatrix; + this->errorModel.setPropMatrix(propMatrix); +} + +/*! + Getter for `AMatrix` used for error propagation + @return Current matrix +*/ +Eigen::MatrixXd StarTracker::getAMatrix() const +{ + return this->AMatrix; +} diff --git a/src/simulation/sensors/starTracker/starTracker.h b/src/simulation/sensors/starTracker/starTracker.h index 39ed5ed50a..36bd5622c2 100755 --- a/src/simulation/sensors/starTracker/starTracker.h +++ b/src/simulation/sensors/starTracker/starTracker.h @@ -38,7 +38,7 @@ class StarTracker: public SysModel { public: StarTracker(); ~StarTracker(); - + void UpdateState(uint64_t CurrentSimNanos); void Reset(uint64_t CurrentClock); //!< Method for reseting the module void readInputMessages(); @@ -47,9 +47,12 @@ class StarTracker: public SysModel { void applySensorErrors(); void computeTrueOutput(); void computeQuaternion(double *sigma, STSensorMsgPayload *sensorValue); - + + void setAMatrix(const Eigen::MatrixXd& propMatrix); + Eigen::MatrixXd getAMatrix() const; + public: - + uint64_t sensorTimeTag; //!< [ns] Current time tag for sensor out ReadFunctor scStateInMsg; //!< [-] sc input state message Message sensorOutMsg; //!< [-] sensor output state message From d99b5dfb45d87ec5fcedccbc57ce4b9f7f48c40d Mon Sep 17 00:00:00 2001 From: Natsoulas Date: Sat, 21 Dec 2024 11:36:49 -0800 Subject: [PATCH 4/7] simpleVoltEstimator: added setter + getter, initialize noise model in constructor --- .../simpleVoltEstimator.cpp | 44 ++++++++++++++++--- .../simpleVoltEstimator/simpleVoltEstimator.h | 5 ++- 2 files changed, 41 insertions(+), 8 deletions(-) diff --git a/src/simulation/sensors/simpleVoltEstimator/simpleVoltEstimator.cpp b/src/simulation/sensors/simpleVoltEstimator/simpleVoltEstimator.cpp index 526e918fbd..7851201eaf 100755 --- a/src/simulation/sensors/simpleVoltEstimator/simpleVoltEstimator.cpp +++ b/src/simulation/sensors/simpleVoltEstimator/simpleVoltEstimator.cpp @@ -27,11 +27,20 @@ SimpleVoltEstimator::SimpleVoltEstimator() { this->estVoltState = this->voltOutMsg.zeroMsgPayload; this->trueVoltState = this->voltOutMsg.zeroMsgPayload; + + // Initialize matrices with proper sizes and default values this->PMatrix.resize(1,1); this->PMatrix.fill(0.0); this->walkBounds.resize(1); this->walkBounds.fill(0.0); - this->errorModel = GaussMarkov(1, this->RNGSeed); + this->AMatrix.resize(1,1); + this->AMatrix.setIdentity(1,1); + + // Initialize noise model and set default parameters + this->errorModel = GaussMarkov(1, this->RNGSeed); + this->errorModel.setNoiseMatrix(this->PMatrix); + this->errorModel.setUpperBounds(this->walkBounds); + this->errorModel.setPropMatrix(this->AMatrix); } /*! Destructor. Nothing here. */ @@ -57,20 +66,18 @@ void SimpleVoltEstimator::Reset(uint64_t CurrentSimNanos) int64_t numStates = 1; - //! - Initialize the propagation matrix to default values for use in update - this->AMatrix.setIdentity(numStates, numStates); - //! - Alert the user and stop if the noise matrix is the wrong size. That'd be bad. if (this->PMatrix.size() != numStates*numStates) { bskLogger.bskLog(BSK_ERROR, "Your process noise matrix (PMatrix) is not %ld*%ld. Size is %ld. Quitting", numStates, numStates, this->PMatrix.size()); return; } - //! - Set the matrices of the lower level error propagation (GaussMarkov) - this->errorModel.setNoiseMatrix(this->PMatrix); - this->errorModel.setRNGSeed(this->RNGSeed); if (this->walkBounds.size() != numStates) { bskLogger.bskLog(BSK_ERROR, "Your walkbounds vector is not %ld elements. Quitting", numStates); } + + //! - Update the noise model parameters + this->errorModel.setNoiseMatrix(this->PMatrix); + this->errorModel.setRNGSeed(this->RNGSeed); this->errorModel.setUpperBounds(this->walkBounds); } @@ -122,3 +129,26 @@ void SimpleVoltEstimator::UpdateState(uint64_t CurrentSimNanos) this->applyErrors(); this->writeOutputMessages(CurrentSimNanos); } + +/*! + Setter for `AMatrix` used for error propagation + @param propMatrix Matrix to set +*/ +void SimpleVoltEstimator::setAMatrix(const Eigen::MatrixXd& propMatrix) +{ + if(propMatrix.rows() != 1 || propMatrix.cols() != 1) { + bskLogger.bskLog(BSK_ERROR, "SimpleVoltEstimator: Propagation matrix must be 1x1"); + return; + } + this->AMatrix = propMatrix; + this->errorModel.setPropMatrix(propMatrix); +} + +/*! + Getter for `AMatrix` used for error propagation + @return Current matrix +*/ +Eigen::MatrixXd SimpleVoltEstimator::getAMatrix() const +{ + return this->AMatrix; +} diff --git a/src/simulation/sensors/simpleVoltEstimator/simpleVoltEstimator.h b/src/simulation/sensors/simpleVoltEstimator/simpleVoltEstimator.h index 5a5a4064c2..093fe56680 100755 --- a/src/simulation/sensors/simpleVoltEstimator/simpleVoltEstimator.h +++ b/src/simulation/sensors/simpleVoltEstimator/simpleVoltEstimator.h @@ -41,6 +41,9 @@ class SimpleVoltEstimator: public SysModel { void readInputMessages(); void writeOutputMessages(uint64_t Clock); + void setAMatrix(const Eigen::MatrixXd& propMatrix); + Eigen::MatrixXd getAMatrix() const; + public: Eigen::MatrixXd PMatrix; //!< -- Cholesky-decomposition or matrix square root of the covariance matrix to apply errors with Eigen::VectorXd walkBounds; //!< -- "3-sigma" errors to permit for states @@ -49,7 +52,7 @@ class SimpleVoltEstimator: public SysModel { VoltMsgPayload trueVoltState; //!< -- voltage state without errors VoltMsgPayload estVoltState; //!< -- voltage state including errors BSKLogger bskLogger; //!< -- BSK Logging - + ReadFunctor voltInMsg; //!< voltage input msg private: From 16be932865493abab84a5f48b56a2c73ff367387 Mon Sep 17 00:00:00 2001 From: Natsoulas Date: Sat, 21 Dec 2024 11:39:41 -0800 Subject: [PATCH 5/7] Magnetometer refactor: initialize noise model in constructor, update reset method --- .../sensors/magnetometer/magnetometer.cpp | 29 +++++++++++++++++-- 1 file changed, 27 insertions(+), 2 deletions(-) diff --git a/src/simulation/sensors/magnetometer/magnetometer.cpp b/src/simulation/sensors/magnetometer/magnetometer.cpp index 87355fb6e9..9e95324f8f 100644 --- a/src/simulation/sensors/magnetometer/magnetometer.cpp +++ b/src/simulation/sensors/magnetometer/magnetometer.cpp @@ -30,7 +30,23 @@ Magnetometer::Magnetometer() this->senBias.fill(0.0); // Tesla this->senNoiseStd.fill(-1.0); // Tesla this->walkBounds.fill(0.0); - this->noiseModel = GaussMarkov(this->numStates); + + // Initialize noise model + this->noiseModel = GaussMarkov(this->numStates, this->RNGSeed); + + // Initialize noise matrices with defaults + Eigen::MatrixXd nMatrix; + nMatrix.resize(3,3); + nMatrix.setZero(); + this->noiseModel.setNoiseMatrix(nMatrix); + + Eigen::MatrixXd pMatrix; + pMatrix.setIdentity(3,3); + this->noiseModel.setPropMatrix(pMatrix); + + this->noiseModel.setUpperBounds(this->walkBounds); + + // Initialize other parameters this->scaleFactor = 1.0; this->maxOutput = 1e200; // Tesla this->minOutput = -1e200; // Tesla @@ -67,10 +83,19 @@ void Magnetometer::Reset(uint64_t CurrentSimNanos) bskLogger.bskLog(BSK_ERROR, "Spacecraft state message name (stateInMsg) is empty."); } + // Update noise model parameters this->noiseModel.setUpperBounds(this->walkBounds); - auto nMatrix = (this->senNoiseStd).asDiagonal(); + + Eigen::MatrixXd nMatrix; + nMatrix.resize(3,3); + nMatrix.setZero(); + nMatrix(0,0) = this->senNoiseStd(0); + nMatrix(1,1) = this->senNoiseStd(1); + nMatrix(2,2) = this->senNoiseStd(2); this->noiseModel.setNoiseMatrix(nMatrix); this->noiseModel.setRNGSeed(this->RNGSeed); + + // Set saturation bounds Eigen::MatrixXd satBounds; satBounds.resize(this->numStates, 2); satBounds(0, 0) = this->minOutput; From 74aea319b87426a75bf4215143b62922bf3e24f0 Mon Sep 17 00:00:00 2001 From: Natsoulas Date: Sat, 21 Dec 2024 11:41:19 -0800 Subject: [PATCH 6/7] coarseSunSensor refactor: add a prop matrix setter + getter --- .../coarseSunSensor/coarseSunSensor.cpp | 30 +++++++++++++++++++ .../sensors/coarseSunSensor/coarseSunSensor.h | 14 +++++---- 2 files changed, 39 insertions(+), 5 deletions(-) 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 From e04558d19addd718c5b5fe4d799d0bf94280802c Mon Sep 17 00:00:00 2001 From: Natsoulas Date: Sat, 21 Dec 2024 11:41:42 -0800 Subject: [PATCH 7/7] update known issues + release notes --- docs/source/Support/bskKnownIssues.rst | 4 ++++ docs/source/Support/bskReleaseNotes.rst | 7 ++++++- 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/docs/source/Support/bskKnownIssues.rst b/docs/source/Support/bskKnownIssues.rst index de0df7716e..e50eccf49a 100644 --- a/docs/source/Support/bskKnownIssues.rst +++ b/docs/source/Support/bskKnownIssues.rst @@ -29,6 +29,10 @@ Version |release| - If configuring and building Basilisk directly with ``conan install`` and ``build`` commands, the ``-if dist3/conan`` argument is no longer needed. The Basilisk install location is setup with ``conan 2`` arguments inside ``conanfile.py``. +- Sensor noise models were not being initialized correctly in sensor models such as + :ref:`magnetometer` and :ref:`simpleVoltEstimator` modules. This is now fixed in the current release. +- Propagation matrices were private in the :ref:`simpleVoltEstimator` and :ref:`starTracker` modules. + This is now fixed in the current release by the addition of public methods to set and get the propagation matrices. Version 2.5.0 diff --git a/docs/source/Support/bskReleaseNotes.rst b/docs/source/Support/bskReleaseNotes.rst index a9a29995fc..20897b26df 100644 --- a/docs/source/Support/bskReleaseNotes.rst +++ b/docs/source/Support/bskReleaseNotes.rst @@ -100,9 +100,14 @@ Version |release| ``%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.i"`` instead of ``%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.h"``. See ``src/simulation/dynamics/dragEffector/dragDynamicEffector.i`` for an example. - - Update CI Linux build with ``opNav`` to use Ubuntu 22.04, not latest (i.e. 24.02). The latter does not support directly Python 3.11, and Basilisk does not support Python 3.13 yet. +- Resolved inconstencies in sensor noise handling for the :ref:`imuSensor`, :ref:`coarseSunsensor`, + :ref:`magnetometer`, :ref:`starTracker`, and :ref:`simpleVoltEstimator` modules. +- Added setter and getter methods for the propagation matrices in the :ref:`simpleVoltEstimator` + and :ref:`starTracker` modules as their ``Amatrix`` attributes were private. +- Name change warning added to module documentation for the ``imuSensor`` ``walkBounds`` attribute to ``errorBounds`` + and a note on specifying sensor properties in :ref:`scenarioGaussMarkovRandomWalk`. Version 2.5.0 (Sept. 30, 2024)