diff options
author | Johan Jansen <jnsn.johan@gmail.com> | 2015-02-05 13:15:30 +0100 |
---|---|---|
committer | Johan Jansen <jnsn.johan@gmail.com> | 2015-02-11 13:38:59 +0100 |
commit | 022208e998ec6c1594b7493c35d15d5358587d86 (patch) | |
tree | 3c57513a9d879ce51ac62aa2010c63d78d495bac /src | |
parent | ce5a9929ff3748e1c0541388c41e5866d307d1c2 (diff) | |
download | px4-firmware-022208e998ec6c1594b7493c35d15d5358587d86.tar.gz px4-firmware-022208e998ec6c1594b7493c35d15d5358587d86.tar.bz2 px4-firmware-022208e998ec6c1594b7493c35d15d5358587d86.zip |
AttPosEKF: Move initializeParameters() from header to implementation file
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_22states.cpp | 34 | ||||
-rw-r--r-- | src/modules/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(); |