dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf: extract range finder noise computation

This commit is contained in:
bresch 2022-03-15 17:08:29 +01:00 committed by Mathieu Bresciani
parent 97b2947416
commit 064518f57a
2 changed files with 10 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/ekf2/EKF/ekf.h
View File

@ -930,8 +930,9 @@ private:
void updateGroundEffect(); void updateGroundEffect();
// return an estimation of the GPS altitude variance // return an estimation of the sensor altitude variance
float getGpsHeightVariance(); float getGpsHeightVariance();
float getRngHeightVariance() const;
// calculate the measurement variance for the optical flow sensor // calculate the measurement variance for the optical flow sensor
float calcOptFlowMeasVar(); float calcOptFlowMeasVar();

8
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -1361,6 +1361,14 @@ void Ekf::updateBaroHgtBias()
} }
} }
float Ekf::getRngHeightVariance() const
{
const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
const float var = sq(_params.range_noise) + dist_dependant_var;
const float var_sat = fmaxf(var, 0.01f);
return var_sat;
}
void Ekf::updateGroundEffect() void Ekf::updateGroundEffect()
{ {
if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) { if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) {