From 4bbabf8b249f2e96654c782b6c5df111a6e52c37 Mon Sep 17 00:00:00 2001
From: tteil <thibaud.teil@lasp.colorado.edu>
Date: Sat, 9 Nov 2024 20:48:09 -0700
Subject: [PATCH] add ground knowledge outlier detection

pop and add to separate branch later
---
 .../attGuidance/flybyPoint/flybyPoint.cpp     | 40 +++++++++++++++++++
 .../attGuidance/flybyPoint/flybyPoint.h       | 10 ++++-
 2 files changed, 49 insertions(+), 1 deletion(-)

diff --git a/src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.cpp b/src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.cpp
index 6cfd0e30bb..54187778e8 100755
--- a/src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.cpp
+++ b/src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.cpp
@@ -46,6 +46,8 @@ 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->previousFilterPosition = r_BN_N;
+            this->previousFilterVelocity = v_BN_N;
             this->computeFlybyParameters(r_BN_N, v_BN_N);
             this->computeRN(r_BN_N, v_BN_N);
             this->firstRead = false;
@@ -58,6 +60,8 @@ void FlybyPoint::UpdateState(uint64_t currentSimNanos)
 
             /*! update lastFilterReadTime to current time and dt to zero */
             this->lastFilterReadTime = currentSimNanos;
+            this->previousFilterPosition = r_BN_N;
+            this->previousFilterVelocity = v_BN_N;
             this->dt = 0;
         }
     }
@@ -110,6 +114,14 @@ bool FlybyPoint::checkValidity(Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N)
     if (maxPredictedAcceleration > this->maxAcceleration && this->maxAcceleration > 0) {
         valid = false;
     }
+    double deltaPositionNorm = (r_BN_N - (this->previousFilterPosition + this->dt*this->previousFilterVelocity)).norm();
+    double deltaVelocityNorm = (v_BN_N - this->previousFilterVelocity).norm();
+    if (deltaPositionNorm > this->positionKnowledgeSigma && this->positionKnowledgeSigma > 0) {
+        valid = false;
+    }
+    if (deltaVelocityNorm > this->velocityKnowledgeSigma && this->velocityKnowledgeSigma > 0) {
+        valid = false;
+    }
 
     return valid;
 }
@@ -232,3 +244,31 @@ 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;
+}
+
+/*! Get the ground based velocity knowledge standard deviation
+ @return sigma
+ */
+double FlybyPoint::getVelocityKnowledgeSigma() const {
+    return this->velocityKnowledgeSigma;
+}
+
+/*! Set the ground based velocity knowledge sigma
+ @param sigma
+ */
+void FlybyPoint::setVelocityKnowledgeSigma(double velocityKnowledgeStd) {
+    this->velocityKnowledgeSigma = velocityKnowledgeStd;
+}
\ No newline at end of file
diff --git a/src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.h b/src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.h
index 85c2b1c6e1..e3d7049f74 100755
--- a/src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.h
+++ b/src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.h
@@ -55,8 +55,12 @@ class FlybyPoint: public SysModel {
     void setMaximumAccelerationThreshold(double maxAccelerationThreshold);
     double getMaximumRateThreshold() const;
     void setMaximumRateThreshold(double maxRateThreshold);
+    double getPositionKnowledgeSigma() const;
+    void setPositionKnowledgeSigma(double positionKnowledgeStd);
+    double getVelocityKnowledgeSigma() const;
+    void setVelocityKnowledgeSigma(double velocityKnowledgeStd);
 
-    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
 
@@ -74,6 +78,10 @@ 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 previousFilterPosition{};           //!< Last position used to create profile
+    Eigen::Vector3d previousFilterVelocity{};           //!< Last velocity used to create profile
+    double positionKnowledgeSigma = 0;           //!< Last position used to create profile
+    double velocityKnowledgeSigma = 0;           //!< Last velocity used to create profile
 
 };