AttPosEKF: Move initializeParameters() from header to implementation file

2 files changed, 40 insertions, 35 deletions

diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
index d3f3fde7b..44a9f4fb7 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
@@ -158,6 +158,40 @@ AttPosEKF::~AttPosEKF()
 {
 }
 
+void AttPosEKF::InitialiseParameters()
+{
+    covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
+    covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
+    rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
+    EAS2TAS = 1.0f;
+
+    yawVarScale = 1.0f;
+    windVelSigma = 0.1f;
+    dAngBiasSigma = 5.0e-7f;
+    dVelBiasSigma = 1e-4f;
+    magEarthSigma = 3.0e-4f;
+    magBodySigma  = 3.0e-4f;
+
+    vneSigma = 0.2f;
+    vdSigma = 0.3f;
+    posNeSigma = 2.0f;
+    posDSigma = 2.0f;
+
+    magMeasurementSigma = 0.05;
+    airspeedMeasurementSigma = 1.4f;
+    gyroProcessNoise = 1.4544411e-2f;
+    accelProcessNoise = 0.5f;
+
+    gndHgtSigma  = 0.1f; // terrain gradient 1-sigma
+    R_LOS = 0.3f; // optical flow measurement noise variance (rad/sec)^2
+    flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check
+    auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter
+    rngInnovGate = 5.0f; // number of standard deviations applied to the range finder innovation consistency check
+    minFlowRng = 0.3f; //minimum range between ground and flow sensor
+    moCompR_LOS = 0.0; // scaler from sensor gyro rate to uncertainty in LOS rate
+}
+
+
 void AttPosEKF::UpdateStrapdownEquationsNED()
 {
     Vector3f delVelNav;
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h
index a4b95f14a..221a17ba7 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.h
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h
@@ -23,7 +23,7 @@ public:
 
     /*
      * parameters are defined here and initialised in
-     * the InitialiseParameters() (which is just 20 lines down)
+     * the InitialiseParameters()
      */
 
     float covTimeStepMax; // maximum time allowed between covariance predictions
@@ -50,40 +50,6 @@ public:
 
     float EAS2TAS; // ratio f true to equivalent airspeed
 
-    void InitialiseParameters()
-    {
-        covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
-        covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
-        rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
-        EAS2TAS = 1.0f;
-
-        yawVarScale = 1.0f;
-        windVelSigma = 0.1f;
-        dAngBiasSigma = 5.0e-7f;
-        dVelBiasSigma = 1e-4f;
-        magEarthSigma = 3.0e-4f;
-        magBodySigma  = 3.0e-4f;
-
-        vneSigma = 0.2f;
-        vdSigma = 0.3f;
-        posNeSigma = 2.0f;
-        posDSigma = 2.0f;
-
-        magMeasurementSigma = 0.05;
-        airspeedMeasurementSigma = 1.4f;
-        gyroProcessNoise = 1.4544411e-2f;
-        accelProcessNoise = 0.5f;
-
-        gndHgtSigma  = 0.1f; // terrain gradient 1-sigma
-        R_LOS = 0.3f; // optical flow measurement noise variance (rad/sec)^2
-        flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check
-        auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter
-        rngInnovGate = 5.0f; // number of standard deviations applied to the range finder innovation consistency check
-        minFlowRng = 0.3f; //minimum range between ground and flow sensor
-        moCompR_LOS = 0.0; // scaler from sensor gyro rate to uncertainty in LOS rate
-
-    }
-
     struct mag_state_struct {
         unsigned obsIndex;
         float MagPred[3];
@@ -346,6 +312,11 @@ public:
 
 protected:
 
+    /**
+    * @brief Initializes algorithm parameters to safe default values
+    **/
+    void InitialiseParameters();
+
     void updateDtVelPosFilt(float dt);
 
     bool FilterHealthy();