From 022208e998ec6c1594b7493c35d15d5358587d86 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 5 Feb 2015 13:15:30 +0100 Subject: AttPosEKF: Move initializeParameters() from header to implementation file --- .../ekf_att_pos_estimator/estimator_22states.cpp | 34 ++++++++++++++++++ .../ekf_att_pos_estimator/estimator_22states.h | 41 ++++------------------ 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(); -- cgit v1.2.3