From cc9181eeebc7449251b0b90a7c1c5b595e79257e Mon Sep 17 00:00:00 2001 From: tteil Date: Thu, 21 Nov 2024 20:27:16 -0700 Subject: [PATCH 1/3] add outlier rejection using knowledge sigma --- .../attGuidance/flybyPoint/flybyPoint.cpp | 27 +++++++++++++++++-- .../attGuidance/flybyPoint/flybyPoint.h | 10 +++++-- 2 files changed, 33 insertions(+), 4 deletions(-) diff --git a/src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.cpp b/src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.cpp index da4197996..8d0b00b4e 100755 --- a/src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.cpp +++ b/src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.cpp @@ -46,12 +46,15 @@ void FlybyPoint::updateState(uint64_t currentSimNanos) /*! If this is the first read, seed the algorithm with the solution */ auto [r_BN_N, v_BN_N] = this->readRelativeState(); if (this->firstRead){ + this->timeOfFirstRead = currentSimNanos*NANO2SEC; + this->firstNavPosition = r_BN_N; + this->firstNavVelocity = v_BN_N; this->computeFlybyParameters(r_BN_N, v_BN_N); this->computeRN(r_BN_N, v_BN_N); this->firstRead = false; } /*! Protect against bad new solutions by checking validity */ - if (this->checkValidity(r_BN_N, v_BN_N)) { + else if (this->checkValidity(currentSimNanos, r_BN_N, v_BN_N)) { /*! update flyby parameters and guidance frame */ this->computeFlybyParameters(r_BN_N, v_BN_N); this->computeRN(r_BN_N, v_BN_N); @@ -89,7 +92,7 @@ void FlybyPoint::computeFlybyParameters(Eigen::Vector3d &r_BN_N, Eigen::Vector3d this->gamma0 = std::atan(v_BN_N.dot(ur_N) / v_BN_N.dot(ut_N)); } -bool FlybyPoint::checkValidity(Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N) const{ +bool FlybyPoint::checkValidity(uint64_t currentSimNanos, Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N) const{ bool valid = true; Eigen::Vector3d ur_N = r_BN_N.normalized(); Eigen::Vector3d uv_N = v_BN_N.normalized(); @@ -110,6 +113,11 @@ bool FlybyPoint::checkValidity(Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N) if (maxPredictedAcceleration > this->maxAcceleration && this->maxAcceleration > 0) { valid = false; } + double deltaT = currentSimNanos*NANO2SEC - this->timeOfFirstRead; + double deltaPositionNorm = (r_BN_N - (this->firstNavPosition + deltaT*this->firstNavVelocity)).norm(); + if (deltaPositionNorm > this->positionKnowledgeSigma && this->positionKnowledgeSigma > 0) { + valid = false; + } return valid; } @@ -232,3 +240,18 @@ double FlybyPoint::getMaximumRateThreshold() const { void FlybyPoint::setMaximumRateThreshold(double maxRateThreshold) { this->maxRate = maxRateThreshold; } + +/*! Get the ground based positional knowledge standard deviation + @return sigma + */ +double FlybyPoint::getPositionKnowledgeSigma() const { + return this->positionKnowledgeSigma; +} + +/*! Set the ground based positional knowledge sigma + @param sigma + */ +void FlybyPoint::setPositionKnowledgeSigma(double positionKnowledgeStd) { + this->positionKnowledgeSigma = positionKnowledgeStd; +} + diff --git a/src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.h b/src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.h index 1d47c201e..def3d504a 100755 --- a/src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.h +++ b/src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.h @@ -36,7 +36,7 @@ class FlybyPoint: public SysModel { void updateState(uint64_t currentSimNanos) override; std::tuple readRelativeState(); - bool checkValidity(Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N) const; + bool checkValidity(uint64_t currentSimNanos, Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N) const; void computeFlybyParameters(Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N); void computeRN(Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N); std::tuple computeGuidanceSolution() const; @@ -55,13 +55,16 @@ class FlybyPoint: public SysModel { void setMaximumAccelerationThreshold(double maxAccelerationThreshold); double getMaximumRateThreshold() const; void setMaximumRateThreshold(double maxRateThreshold); + double getPositionKnowledgeSigma() const; + void setPositionKnowledgeSigma(double positionKnowledgeStd); - ReadFunctor filterInMsg; //!< input msg relative position w.r.t. asteroid + ReadFunctor filterInMsg; //!< input msg relative position w.r.t. asteroid ReadFunctor asteroidEphemerisInMsg; //!< input asteroid ephemeris msg Message attRefOutMsg; //!< Attitude reference output message private: double dt = 0; //!< current time step between last two updates + double timeOfFirstRead = 0; //!< time of first nav solution read double timeBetweenFilterData = 0; //!< time between two subsequent reads of the filter information double toleranceForCollinearity = 0; //!< tolerance for singular conditions when position and velocity are collinear int signOfOrbitNormalFrameVector = 1; //!< Sign of orbit normal vector to complete reference frame @@ -74,6 +77,9 @@ class FlybyPoint: public SysModel { double gamma0 = 0; //!< flight path angle of the spacecraft at time of read [rad] uint64_t lastFilterReadTime = 0; //!< time of last filter read Eigen::Matrix3d R0N; //!< inertial-to-reference DCM at time of read + Eigen::Vector3d firstNavPosition{}; //!< First position used to create profile + Eigen::Vector3d firstNavVelocity{}; //!< First velocity used to create profile + double positionKnowledgeSigma = 0; //!< Last position used to create profile }; From fa26e7ea29d7867e10c50f0043cd49866748e9b1 Mon Sep 17 00:00:00 2001 From: tteil Date: Fri, 22 Nov 2024 12:40:24 -0700 Subject: [PATCH 2/3] update unit test --- .../attGuidance/flybyPoint/_UnitTest/test_flybyPoint.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/fswAlgorithms/attGuidance/flybyPoint/_UnitTest/test_flybyPoint.py b/src/fswAlgorithms/attGuidance/flybyPoint/_UnitTest/test_flybyPoint.py index 4b0b15da2..0b10ef64d 100755 --- a/src/fswAlgorithms/attGuidance/flybyPoint/_UnitTest/test_flybyPoint.py +++ b/src/fswAlgorithms/attGuidance/flybyPoint/_UnitTest/test_flybyPoint.py @@ -43,8 +43,9 @@ @pytest.mark.parametrize("orbit_normal_sign", [1, -1]) @pytest.mark.parametrize("max_rate", [0, 0.01]) @pytest.mark.parametrize("max_acceleration", [0, 1E-7]) +@pytest.mark.parametrize("pos_knowledge", [0, 1E5]) def test_flybyPoint(show_plots, initial_position, initial_velocity, filter_dt, orbit_normal_sign, max_rate, - max_acceleration): + max_acceleration, pos_knowledge): r""" **Validation Test Description** @@ -76,11 +77,11 @@ def test_flybyPoint(show_plots, initial_position, initial_velocity, filter_dt, o """ # each test method requires a single assert method to be called flybyPointTestFunction(show_plots, initial_position, initial_velocity, filter_dt, orbit_normal_sign, - max_rate, max_acceleration) + max_rate, max_acceleration, pos_knowledge) def flybyPointTestFunction(show_plots, initial_position, initial_velocity, filter_dt, orbit_normal_sign, - max_rate, max_acceleration): + max_rate, max_acceleration, pos_knowledge): # setup simulation environment sim_dt = 10 unit_test_sim = SimulationBaseClass.SimBaseClass() @@ -96,6 +97,7 @@ def flybyPointTestFunction(show_plots, initial_position, initial_velocity, filte flyby_guidance.setSignOfOrbitNormalFrameVector(orbit_normal_sign) flyby_guidance.setMaximumRateThreshold(max_rate) flyby_guidance.setMaximumAccelerationThreshold(max_acceleration) + flyby_guidance.setPositionKnowledgeSigma(pos_knowledge) unit_test_sim.AddModelToTask("unit_task", flyby_guidance) input_data = messaging.NavTransMsgPayload() From 4ed1b3e408ff66714ea7fcaee8056293ea79a1dd Mon Sep 17 00:00:00 2001 From: tteil Date: Fri, 29 Nov 2024 09:27:17 -0700 Subject: [PATCH 3/3] remove trailing white space --- src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.cpp b/src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.cpp index 8d0b00b4e..9e78dc6ed 100755 --- a/src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.cpp +++ b/src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.cpp @@ -254,4 +254,3 @@ double FlybyPoint::getPositionKnowledgeSigma() const { void FlybyPoint::setPositionKnowledgeSigma(double positionKnowledgeStd) { this->positionKnowledgeSigma = positionKnowledgeStd; } -