Merge pull request #362 from lasp/feature/1269-flyby-guidance-updates

Feature/1269 flyby guidance updates

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()

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,17 @@ 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;
+}

src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.h

@@ -36,7 +36,7 @@ class FlybyPoint: public SysModel {
     void updateState(uint64_t currentSimNanos) override;
 
     std::tuple<Eigen::Vector3d, Eigen::Vector3d> 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<Eigen::Vector3d, Eigen::Vector3d, Eigen::Vector3d> 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<NavTransMsgPayload>  filterInMsg;               //!< input msg relative position w.r.t. asteroid
+    ReadFunctor<NavTransMsgPayload> filterInMsg;               //!< input msg relative position w.r.t. asteroid
     ReadFunctor<EphemerisMsgPayload> asteroidEphemerisInMsg;    //!< input asteroid ephemeris msg
     Message<AttRefMsgPayload> 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
 
 };