diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 4dd55399a0..f9697834b4 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -207,8 +207,8 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Param: ALT_M_NSE // @DisplayName: Altitude measurement noise (m) - // @Description: This is the RMS value of noise in the altitude measurement. Increasing it reduces the weighting of the baro measurement and will make the filter respond more slowly to baro measurement errors, but will make it more sensitive to GPS and accelerometer errors. - // @Range: 0.1 10.0 + // @Description: This is the RMS value of noise in the altitude measurement. Increasing it reduces the weighting of the baro measurement and will make the filter respond more slowly to baro measurement errors, but will make it more sensitive to GPS and accelerometer errors. A larger value for EK3_ALT_M_NSE may be required when operating with EK3_SRCx_POSZ = 0. This parameter also sets the noise for the 'synthetic' zero height measurement that is used when EK3_SRCx_POSZ = 0. + // @Range: 0.1 100.0 // @Increment: 0.1 // @User: Advanced // @Units: m diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 721e620041..cadf2bd64a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -1115,7 +1115,10 @@ void NavEKF3_core::selectHeightForFusion() const bool extNavDataIsFresh = (imuSampleTime_ms - extNavMeasTime_ms < 500); #endif // select height source - if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::RANGEFINDER) && _rng && rangeFinderDataIsFresh) { + if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::NONE)) { + // user has specified no height sensor + activeHgtSource = AP_NavEKF_Source::SourceZ::NONE; + } else if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::RANGEFINDER) && _rng && rangeFinderDataIsFresh) { // user has specified the range finder as a primary height source activeHgtSource = AP_NavEKF_Source::SourceZ::RANGEFINDER; } else if ((frontend->_useRngSwHgt > 0) && ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BARO) || (frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS)) && _rng && rangeFinderDataIsFresh) { @@ -1269,6 +1272,18 @@ void NavEKF3_core::selectHeightForFusion() posDownObsNoise *= frontend->gndEffectBaroScaler; } velPosObs[5] = -hgtMea; + } else if ((activeHgtSource == AP_NavEKF_Source::SourceZ::NONE && imuSampleTime_ms - lastHgtPassTime_ms > 70)) { + // fuse a constant height of 0 at 14 Hz + hgtMea = 0.0f; + fuseHgtData = true; + velPosObs[5] = -hgtMea; + if (onGround) { + // use a typical vertical positoin observation noise when not flying for faster IMU delta velocity bias estimation + posDownObsNoise = sq(2.0f); + } else { + // alow a larger value when flying to accomodate vertical maneouvres + posDownObsNoise = sq(constrain_ftype(frontend->_baroAltNoise, 2.0f, 100.0f)); + } } else { fuseHgtData = false; }