diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 1a644e4eb6..9f23d2d7f7 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -909,8 +909,8 @@ void Ekf::controlHeightFusion() } // Fuse range finder data if available - const bool use_for_hagl = _terrain_initialised && shouldUseRangeFinderForHagl(); - if ((use_for_hagl || _control_status.flags.rng_hgt) && _range_sensor.isDataHealthy()) { + controlHaglFusion(); + if ((_control_status.flags.rng_hgt || _hagl_sensor_status.flags.range_finder) && _range_sensor.isDataHealthy()) { fuseHaglAllStates(); } } @@ -922,7 +922,7 @@ void Ekf::checkRangeAidSuitability() && isTerrainEstimateValid()) { // check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching // Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice - const float range_hagl = _terrain_vpos - _state.pos(2); + const float range_hagl = _state.posd_terrain - _state.pos(2); const float range_hagl_max = _is_range_aid_suitable ? _params.max_hagl_for_range_aid : (_params.max_hagl_for_range_aid * 0.7f); const bool is_in_range = range_hagl < range_hagl_max; diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index f6487c472f..649e5c4bf0 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -865,7 +865,7 @@ void Ekf::predictCovariance() } - if (_terrain_initialised) { + if (_params.terrain_fusion_mode > 0) { nextP(0,24) = P(0,24) - P(1,24)*PS11 + P(10,24)*PS6 + P(11,24)*PS7 + P(12,24)*PS9 - P(2,24)*PS12 - P(3,24)*PS13; nextP(1,24) = P(0,24)*PS11 + P(1,24) - P(10,24)*PS34 + P(11,24)*PS9 - P(12,24)*PS7 + P(2,24)*PS13 - P(3,24)*PS12; nextP(2,24) = P(0,24)*PS12 - P(1,24)*PS13 - P(10,24)*PS9 - P(11,24)*PS34 + P(12,24)*PS6 + P(2,24) + P(3,24)*PS11; @@ -1093,17 +1093,12 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) } // terrain vertical position - if (!_terrain_initialised) { - P.uncorrelateCovarianceSetVariance<1>(24, 0.0f); + // constrain variances + P(24, 24) = math::constrain(P(24, 24), 0.0f, P_lim[8]); - } else { - // constrain variances - P(24, 24) = math::constrain(P(24, 24), 0.0f, P_lim[8]); - - // force symmetry - if (force_symmetry) { - P.makeRowColSymmetric<1>(24); - } + // force symmetry + if (force_symmetry) { + P.makeRowColSymmetric<1>(24); } } diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 534b4acdd3..d1bbb0d22e 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -108,9 +108,6 @@ bool Ekf::update() // control fusion of observation data controlFusionModes(); - // run a separate filter for terrain estimation - runTerrainEstimator(); - updated = true; // run EKF-GSF yaw estimator diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index b7c64ea4db..771e362369 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -228,13 +228,13 @@ public: uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; } // get the estimated terrain vertical position relative to the NED origin - float getTerrainVertPos() const { return _terrain_vpos; }; + float getTerrainVertPos() const { return _state.posd_terrain; }; // get the number of times the vertical terrain position has been reset uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; }; // get the terrain variance - float get_terrain_var() const { return _terrain_var; } + float get_terrain_var() const { return P(24,24); } Vector3f getGyroBias() const { return _state.delta_ang_bias / _dt_ekf_avg; } // get the gyroscope bias in rad/s Vector3f getAccelBias() const { return _state.delta_vel_bias / _dt_ekf_avg; } // get the accelerometer bias in m/s**2 @@ -534,10 +534,9 @@ private: Vector3f _prev_dvel_bias_var{}; ///< saved delta velocity XYZ bias variances (m/sec)**2 // Terrain height state estimation - float _terrain_vpos{0.0f}; ///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m) - float _terrain_var{1e4f}; ///< variance of terrain position estimate (m**2) - uint8_t _terrain_vpos_reset_counter{0}; ///< number of times _terrain_vpos has been reset - uint64_t _time_last_hagl_fuse{0}; ///< last system time that a range sample was fused by the terrain estimator + uint8_t _terrain_vpos_reset_counter{0}; ///< number of times _state.posd_terrain has been reset + uint64_t _time_last_hagl_fuse{0}; ///< last system time that a range sample was fused by the terrain estimator + uint64_t _time_last_fake_hagl_fuse{0}; ///< last system time that a fake range sample was fused by the terrain estimator bool _hagl_valid{false}; ///< true when the height above ground estimate is valid terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground @@ -690,12 +689,10 @@ private: // initialise the terrain vertical position estimator void initHagl(); - void runTerrainEstimator(); - void predictHagl(); + void controlHaglFusion(); // update the terrain vertical position estimate using a height above ground measurement from the range finder void controlHaglRngFusion(); - void fuseHaglRng(); void startHaglRngFusion(); void resetHaglRngIfNeeded(); void resetHaglRng(); @@ -707,7 +704,6 @@ private: void startHaglFlowFusion(); void resetHaglFlow(); void stopHaglFlowFusion(); - void fuseFlowForTerrain(); void controlHaglFakeFusion(); void resetHaglFake(); @@ -848,7 +844,7 @@ private: bool otherHeadingSourcesHaveStopped(); void checkHaglYawResetReq(); - float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; } + float getTerrainVPos() const { return isTerrainEstimateValid() ? _state.posd_terrain : _last_on_ground_posD; } void runOnGroundYawReset(); bool isYawResetAuthorized() const { return !_is_yaw_fusion_inhibited; } diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 1fdb901675..d9d3b55029 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -74,7 +74,7 @@ void Ekf::resetHorizontalVelocityToOpticalFlow() _information_events.flags.reset_vel_to_flow = true; ECL_INFO("reset velocity to flow"); // constrain height above ground to be above minimum possible - const float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance); + const float heightAboveGndEst = fmaxf((_state.posd_terrain - _state.pos(2)), _params.rng_gnd_clearance); // calculate absolute distance from focal point to centre of frame assuming a flat earth const float range = heightAboveGndEst / _range_sensor.getCosTilt(); @@ -781,7 +781,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const if (_control_status.flags.opt_flow) { float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f); - vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * _flow_innov.norm(); + vel_err_conservative = math::max((_state.posd_terrain - _state.pos(2)), gndclearance) * _flow_innov.norm(); } if (_control_status.flags.gps) { @@ -818,7 +818,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl // Calculate optical flow limits // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion - const float flow_vxy_max = fmaxf(0.5f * _flow_max_rate * (_terrain_vpos - _state.pos(2)), 0.0f); + const float flow_vxy_max = fmaxf(0.5f * _flow_max_rate * (_state.posd_terrain - _state.pos(2)), 0.0f); const float flow_hagl_min = _flow_min_distance; const float flow_hagl_max = _flow_max_distance; @@ -1304,7 +1304,7 @@ void Ekf::startRngAidHgtFusion() // calculate height sensor offset such that current // measurement matches our current height estimate - _hgt_sensor_offset = _terrain_vpos; + _hgt_sensor_offset = _state.posd_terrain; } } diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index e695feb775..e57468858c 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -86,7 +86,7 @@ void Ekf::fuseOptFlow() const Vector3f vel_body = earth_to_body * vel_rel_earth; // height above ground of the IMU - float heightAboveGndEst = _terrain_vpos - _state.pos(2); + float heightAboveGndEst = _state.posd_terrain - _state.pos(2); // calculate the sensor position relative to the IMU in earth frame const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index 399992ef1d..882d8906e5 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -48,7 +48,7 @@ void Ekf::initHagl() resetHaglFake(); } -void Ekf::runTerrainEstimator() +void Ekf::controlHaglFusion() { // If we are on ground, store the local position and time to use as a reference if (!_control_status.flags.in_air) { @@ -56,35 +56,18 @@ void Ekf::runTerrainEstimator() _control_status.flags.rng_fault = false; } - predictHagl(); - controlHaglRngFusion(); controlHaglFlowFusion(); controlHaglFakeFusion(); - // constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2) - if (_terrain_vpos - _state.pos(2) < _params.rng_gnd_clearance) { - _terrain_vpos = _params.rng_gnd_clearance + _state.pos(2); + // constrain terrain position to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2) + if (_state.posd_terrain - _state.pos(2) < _params.rng_gnd_clearance) { + _state.posd_terrain = _params.rng_gnd_clearance + _state.pos(2); } updateTerrainValidity(); } -void Ekf::predictHagl() -{ - // predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle - - // process noise due to errors in vehicle height estimate - _terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise); - - // process noise due to terrain gradient - _terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient) - * (sq(_state.vel(0)) + sq(_state.vel(1))); - - // limit the variance to prevent it becoming badly conditioned - _terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f); -} - void Ekf::controlHaglRngFusion() { if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder) @@ -103,7 +86,7 @@ void Ekf::controlHaglRngFusion() if (_hagl_sensor_status.flags.range_finder) { if (continuing_conditions_passing) { - fuseHaglRng(); + /* fuseHaglRng(); */ // done when fusing range finder // We have been rejecting range data for too long const uint64_t timeout = static_cast(_params.terrain_timeout * 1e6f); @@ -153,11 +136,11 @@ void Ekf::resetHaglRngIfNeeded() { if (_hagl_sensor_status.flags.flow) { const float meas_hagl = _range_sensor.getDistBottom(); - const float pred_hagl = _terrain_vpos - _state.pos(2); + const float pred_hagl = _state.posd_terrain - _state.pos(2); const float hagl_innov = pred_hagl - meas_hagl; const float obs_variance = getRngVar(); - const float hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); + const float hagl_innov_var = fmaxf(P(24, 24) + obs_variance, obs_variance); const float gate_size = fmaxf(_params.range_innov_gate, 1.0f); const float hagl_test_ratio = sq(hagl_innov) / (sq(gate_size) * hagl_innov_var); @@ -168,7 +151,7 @@ void Ekf::resetHaglRngIfNeeded() resetHaglRng(); } else { - fuseHaglRng(); + /* fuseHaglRng(); */ } } else { @@ -185,8 +168,8 @@ float Ekf::getRngVar() void Ekf::resetHaglRng() { - _terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom(); - _terrain_var = getRngVar(); + _state.posd_terrain = _state.pos(2) + _range_sensor.getDistBottom(); + P(24, 24) = getRngVar(); _terrain_vpos_reset_counter++; _time_last_hagl_fuse = _time_last_imu; } @@ -196,43 +179,6 @@ void Ekf::stopHaglRngFusion() _hagl_sensor_status.flags.range_finder = false; } -void Ekf::fuseHaglRng() -{ - // get a height above ground measurement from the range finder assuming a flat earth - const float meas_hagl = _range_sensor.getDistBottom(); - - // predict the hagl from the vehicle position and terrain height - const float pred_hagl = _terrain_vpos - _state.pos(2); - - // calculate the innovation - _hagl_innov = pred_hagl - meas_hagl; - - // calculate the observation variance adding the variance of the vehicles own height uncertainty - const float obs_variance = getRngVar(); - - // calculate the innovation variance - limiting it to prevent a badly conditioned fusion - _hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); - - // perform an innovation consistency check and only fuse data if it passes - const float gate_size = fmaxf(_params.range_innov_gate, 1.0f); - _hagl_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var); - - if (_hagl_test_ratio <= 1.0f) { - // calculate the Kalman gain - const float gain = _terrain_var / _hagl_innov_var; - // correct the state - _terrain_vpos -= gain * _hagl_innov; - // correct the variance - _terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f); - // record last successful fusion event - _time_last_hagl_fuse = _time_last_imu; - _innov_check_fail_status.flags.reject_hagl = false; - - } else { - _innov_check_fail_status.flags.reject_hagl = true; - } -} - void Ekf::controlHaglFlowFusion() { if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow)) { @@ -252,8 +198,8 @@ void Ekf::controlHaglFlowFusion() if (continuing_conditions_passing) { // TODO: wait until the midpoint of the flow sample has fallen behind the fusion time horizon - fuseFlowForTerrain(); - _flow_data_ready = false; + /* fuseFlowForTerrain(); */ + /* _flow_data_ready = false; */ // done in flow fusion // TODO: do something when failing continuously the innovation check /* const bool is_fusion_failing = isTimedOut(_time_last_flow_terrain_fuse, _params.reset_timeout_max); */ @@ -283,8 +229,8 @@ void Ekf::startHaglFlowFusion() { _hagl_sensor_status.flags.flow = true; // TODO: do a reset instead of trying to fuse the data? - fuseFlowForTerrain(); - _flow_data_ready = false; + /* fuseFlowForTerrain(); */ + /* _flow_data_ready = false; */ } void Ekf::stopHaglFlowFusion() @@ -295,111 +241,11 @@ void Ekf::stopHaglFlowFusion() void Ekf::resetHaglFlow() { // TODO: use the flow data - _terrain_vpos = fmaxf(0.0f, _state.pos(2)); - _terrain_var = 100.0f; + _state.posd_terrain = fmaxf(0.0f, _state.pos(2)); + P(24, 24) = 100.0f; _terrain_vpos_reset_counter++; } -void Ekf::fuseFlowForTerrain() -{ - // calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed - // correct for gyro bias errors in the data used to do the motion compensation - // Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis. - const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt + Vector2f(_flow_gyro_bias); - - // get latest estimated orientation - const float q0 = _state.quat_nominal(0); - const float q1 = _state.quat_nominal(1); - const float q2 = _state.quat_nominal(2); - const float q3 = _state.quat_nominal(3); - - // calculate the optical flow observation variance - const float R_LOS = calcOptFlowMeasVar(); - - // get rotation matrix from earth to body - const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal); - - // calculate the sensor position relative to the IMU - const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body; - - // calculate the velocity of the sensor relative to the imu in body frame - // Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign - const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt) % pos_offset_body; - - // calculate the velocity of the sensor in the earth frame - const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body; - - // rotate into body frame - const Vector3f vel_body = earth_to_body * vel_rel_earth; - - const float t0 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; - - // constrain terrain to minimum allowed value and predict height above ground - _terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2)); - const float pred_hagl_inv = 1.f / (_terrain_vpos - _state.pos(2)); - - // Calculate observation matrix for flow around the vehicle x axis - const float Hx = vel_body(1) * t0 * pred_hagl_inv * pred_hagl_inv; - - // Constrain terrain variance to be non-negative - _terrain_var = fmaxf(_terrain_var, 0.0f); - - // Cacluate innovation variance - _flow_innov_var(0) = Hx * Hx * _terrain_var + R_LOS; - - // calculate the kalman gain for the flow x measurement - const float Kx = _terrain_var * Hx / _flow_innov_var(0); - - // calculate prediced optical flow about x axis - const float pred_flow_x = vel_body(1) * earth_to_body(2, 2) * pred_hagl_inv; - - // calculate flow innovation (x axis) - _flow_innov(0) = pred_flow_x - opt_flow_rate(0); - - // calculate correction term for terrain variance - const float KxHxP = Kx * Hx * _terrain_var; - - // innovation consistency check - const float gate_size = fmaxf(_params.flow_innov_gate, 1.0f); - float flow_test_ratio = sq(_flow_innov(0)) / (sq(gate_size) * _flow_innov_var(0)); - - // do not perform measurement update if badly conditioned - if (flow_test_ratio <= 1.0f) { - _terrain_vpos += Kx * _flow_innov(0); - // guard against negative variance - _terrain_var = fmaxf(_terrain_var - KxHxP, 0.0f); - _time_last_flow_terrain_fuse = _time_last_imu; - } - - // Calculate observation matrix for flow around the vehicle y axis - const float Hy = -vel_body(0) * t0 * pred_hagl_inv * pred_hagl_inv; - - // Calculuate innovation variance - _flow_innov_var(1) = Hy * Hy * _terrain_var + R_LOS; - - // calculate the kalman gain for the flow y measurement - const float Ky = _terrain_var * Hy / _flow_innov_var(1); - - // calculate prediced optical flow about y axis - const float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) * pred_hagl_inv; - - // calculate flow innovation (y axis) - _flow_innov(1) = pred_flow_y - opt_flow_rate(1); - - // calculate correction term for terrain variance - const float KyHyP = Ky * Hy * _terrain_var; - - // innovation consistency check - flow_test_ratio = sq(_flow_innov(1)) / (sq(gate_size) * _flow_innov_var(1)); - - if (flow_test_ratio <= 1.0f) { - _terrain_vpos += Ky * _flow_innov(1); - // guard against negative variance - _terrain_var = fmaxf(_terrain_var - KyHyP, 0.0f); - _time_last_flow_terrain_fuse = _time_last_imu; - } -} - void Ekf::controlHaglFakeFusion() { if (!_control_status.flags.in_air @@ -412,9 +258,9 @@ void Ekf::controlHaglFakeFusion() void Ekf::resetHaglFake() { // assume a ground clearance - _terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance; + _state.posd_terrain = _state.pos(2) + _params.rng_gnd_clearance; // use the ground clearance value as our uncertainty - _terrain_var = sq(_params.rng_gnd_clearance); + P(24, 24) = sq(_params.rng_gnd_clearance); _time_last_hagl_fuse = _time_last_imu; } @@ -436,7 +282,7 @@ void Ekf::fuseHaglAllStates() const float meas_hagl = _range_sensor.getDistBottom(); // predict the hagl from the vehicle position and terrain height - const float pred_hagl = _terrain_vpos - _state.pos(2); + const float pred_hagl = _state.posd_terrain - _state.pos(2); // calculate the innovation _hagl_innov = pred_hagl - meas_hagl; @@ -456,23 +302,25 @@ void Ekf::fuseHaglAllStates() bool is_fused = false; if (_hagl_test_ratio <= 1.0f) { // calculate the Kalman gain - const float HK0 = 1.0F/_hagl_innov_var; + const float HK0 = 1.0f/_hagl_innov_var; // calculate the observation Jacobians and Kalman gains - SparseVector25f<9,24> Hfusion; // Optical flow observation Jacobians + SparseVector25f<9,24> Hfusion; Vector25f Kfusion; if (_control_status.flags.rng_hgt) { Hfusion.at<9>() = -1.0f; - if (shouldUseRangeFinderForHagl()) { + if (_hagl_sensor_status.flags.range_finder || _hagl_sensor_status.flags.flow) { for (uint8_t index=0; index<=23; index++) { Kfusion(index) = HK0*(P(index,24) - P(index,9)); } + } else { for (uint8_t index=0; index<=23; index++) { Kfusion(index) = - HK0*P(index,9); } } + } else { for (unsigned row=0; row<=23; row++) { // update of all states other than terrain is inhibited @@ -480,44 +328,40 @@ void Ekf::fuseHaglAllStates() } } - if (shouldUseRangeFinderForHagl()) { + if (_hagl_sensor_status.flags.range_finder) { Hfusion.at<24>() = 1.0f; + if (_control_status.flags.rng_hgt) { Kfusion(24) = HK0*(P(24,24) - P(24,9)); + } else { Kfusion(24) = HK0*P(24,24); } + } else { Kfusion(24) = 0.0f; } - is_fused = measurementUpdate(Kfusion, Hfusion, _hagl_innov); } if (is_fused) { // record last successful fusion event - if (shouldUseRangeFinderForHagl()) { + if (_hagl_sensor_status.flags.range_finder) { _time_last_hagl_fuse = _time_last_imu; _innov_check_fail_status.flags.reject_hagl = false; } + if (_control_status.flags.rng_hgt) { _time_last_hgt_fuse = _time_last_imu; _innov_check_fail_status.flags.reject_ver_pos = false; } + } else { - if (shouldUseRangeFinderForHagl()) { - // If we have been rejecting range data for too long, reset to measurement - const uint64_t timeout = static_cast(_params.terrain_timeout * 1e6f); - if (isTimedOut(_time_last_hagl_fuse, timeout)) { - _state.posd_terrain = _state.pos(2) + meas_hagl; - P.uncorrelateCovarianceSetVariance<1>(24, 0.0f); - P(24,24) = obs_variance; - _terrain_vpos_reset_counter++; - } else { - _innov_check_fail_status.flags.reject_hagl = true; - } + if (_hagl_sensor_status.flags.range_finder) { + _innov_check_fail_status.flags.reject_hagl = true; } + if (_control_status.flags.rng_hgt) { _innov_check_fail_status.flags.reject_ver_pos = true; }