diff --git a/EKF/RingBuffer.h b/EKF/RingBuffer.h index 0529aa16c6..ecdf63d8e0 100644 --- a/EKF/RingBuffer.h +++ b/EKF/RingBuffer.h @@ -45,7 +45,8 @@ template class RingBuffer { public: - RingBuffer() { + RingBuffer() + { if (allocate(1)) { // initialize with one empty sample data_type d = {}; @@ -81,17 +82,19 @@ public: for (uint8_t index = 0; index < _size; index++) { _buffer[index] = {}; } + _first_write = true; return true; } - void unallocate() { + void unallocate() + { delete[] _buffer; _buffer = nullptr; } - void push(const data_type& sample) + void push(const data_type &sample) { uint8_t head_new = _head; @@ -115,12 +118,12 @@ public: data_type &operator[](const uint8_t index) { return _buffer[index]; } - const data_type& get_newest() { return _buffer[_head]; } - const data_type& get_oldest() { return _buffer[_tail]; } + const data_type &get_newest() { return _buffer[_head]; } + const data_type &get_oldest() { return _buffer[_tail]; } uint8_t get_oldest_index() const { return _tail; } - bool pop_first_older_than(const uint64_t& timestamp, data_type *sample) + bool pop_first_older_than(const uint64_t ×tamp, data_type *sample) { // start looking from newest observation data for (uint8_t i = 0; i < _size; i++) { diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index cf31b4810c..b13500d880 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -121,6 +121,7 @@ void Ekf::fuseAirspeed() for (unsigned row = 0; row <= 21; row++) { Kfusion[row] = 0.0f; } + } else { // we have no other source of aiding, so use airspeed measurements to correct states Kfusion[0] = SK_TAS[0]*(P[0][4]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][5]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][6]*vd*SH_TAS[0]); @@ -164,6 +165,7 @@ void Ekf::fuseAirspeed() if (_tas_test_ratio > 1.0f) { _innov_check_fail_status.flags.reject_airspeed = true; return; + } else { _innov_check_fail_status.flags.reject_airspeed = false; } @@ -176,6 +178,7 @@ void Ekf::fuseAirspeed() // then calculate P - KHP float KHP[_k_num_states][_k_num_states]; float KH[5]; + for (unsigned row = 0; row < _k_num_states; row++) { KH[0] = Kfusion[row] * H_TAS[4]; @@ -198,11 +201,12 @@ void Ekf::fuseAirspeed() // the covariance marix is unhealthy and must be corrected bool healthy = true; _fault_status.flags.bad_airspeed = false; + for (int i = 0; i < _k_num_states; i++) { if (P[i][i] < KHP[i][i]) { // zero rows and columns - zeroRows(P,i,i); - zeroCols(P,i,i); + zeroRows(P, i, i); + zeroCols(P, i, i); //flag as unhealthy healthy = false; diff --git a/EKF/control.cpp b/EKF/control.cpp index 38a1406973..8a18c0dec9 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -64,15 +64,19 @@ void Ekf::controlFusionModes() // send alignment status message to the console if (_control_status.flags.baro_hgt) { - ECL_INFO("EKF aligned, (pressure height, IMU buf: %i, OBS buf: %i)",(int)_imu_buffer_length,(int)_obs_buffer_length); + ECL_INFO("EKF aligned, (pressure height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length); + } else if (_control_status.flags.ev_hgt) { - ECL_INFO("EKF aligned, (EV height, IMU buf: %i, OBS buf: %i)",(int)_imu_buffer_length,(int)_obs_buffer_length); + ECL_INFO("EKF aligned, (EV height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length); + } else if (_control_status.flags.gps_hgt) { - ECL_INFO("EKF aligned, (GPS height, IMU buf: %i, OBS buf: %i)",(int)_imu_buffer_length,(int)_obs_buffer_length); + ECL_INFO("EKF aligned, (GPS height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length); + } else if (_control_status.flags.rng_hgt) { - ECL_INFO("EKF aligned, (range height, IMU buf: %i, OBS buf: %i)",(int)_imu_buffer_length,(int)_obs_buffer_length); + ECL_INFO("EKF aligned, (range height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length); + } else { - ECL_ERR("EKF aligned, (unknown height, IMU buf: %i, OBS buf: %i)",(int)_imu_buffer_length,(int)_obs_buffer_length); + ECL_ERR("EKF aligned, (unknown height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length); } } @@ -80,13 +84,13 @@ void Ekf::controlFusionModes() } // check faultiness (before pop_first_older_than) to see if we can change back to original height sensor - const baroSample& baro_init = _baro_buffer.get_newest(); + const baroSample &baro_init = _baro_buffer.get_newest(); _baro_hgt_faulty = !((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); - const gpsSample& gps_init = _gps_buffer.get_newest(); + const gpsSample &gps_init = _gps_buffer.get_newest(); _gps_hgt_faulty = !((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL); - const rangeSample& rng_init = _range_buffer.get_newest(); + const rangeSample &rng_init = _range_buffer.get_newest(); _rng_hgt_faulty = !((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL); // check for arrival of new sensor data at the fusion time horizon @@ -105,12 +109,12 @@ void Ekf::controlFusionModes() // calculate 2,2 element of rotation matrix from sensor frame to earth frame _R_rng_to_earth_2_2 = _R_to_earth(2, 0) * _sin_tilt_rng + _R_to_earth(2, 2) * _cos_tilt_rng; _range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed) - && (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt); + && (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt); checkForStuckRange(); _flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed) - && (_R_to_earth(2, 2) > 0.7071f); + && (_R_to_earth(2, 2) > 0.7071f); _ev_data_ready = _ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed); _tas_data_ready = _airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed); @@ -147,13 +151,17 @@ void Ekf::controlExternalVisionFusion() // if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames // needs to be calculated and the observations rotated into the EKF frame of reference - if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !(_params.fusion_mode & MASK_USE_EVYAW)) { + if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) + && !(_params.fusion_mode & MASK_USE_EVYAW)) { + // rotate EV measurements into the EKF Navigation frame calcExtVisRotMat(); } // external vision position aiding selection logic - if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) { + if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align + && _control_status.flags.yaw_align) { + // check for a exernal vision measurement that has fallen behind the fusion time horizon if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { // turn on use of external vision measurements for position @@ -183,7 +191,7 @@ void Ekf::controlExternalVisionFusion() Eulerf euler_init(q_init); // get initial yaw from the observation quaternion - const extVisionSample& ev_newest = _ext_vision_buffer.get_newest(); + const extVisionSample &ev_newest = _ext_vision_buffer.get_newest(); Quatf q_obs(ev_newest.quat); Eulerf euler_obs(q_obs); euler_init(2) = euler_obs(2); @@ -251,12 +259,13 @@ void Ekf::controlExternalVisionFusion() // Use an incremental position fusion method for EV data if using GPS or if the observations are not in NED if (_control_status.flags.gps || (_params.fusion_mode & MASK_ROTATE_EV)) { _fuse_hpos_as_odom = true; + } else { _fuse_hpos_as_odom = false; } - if(_fuse_hpos_as_odom) { - if(!_hpos_prev_available) { + if (_fuse_hpos_as_odom) { + if (!_hpos_prev_available) { // no previous observation available to calculate position change _fuse_pos = false; _hpos_prev_available = true; @@ -292,6 +301,7 @@ void Ekf::controlExternalVisionFusion() if (_time_last_imu - _time_last_of_fuse > (uint64_t)1E6) { resetVelocity(); } + resetPosition(); } } @@ -316,7 +326,10 @@ void Ekf::controlExternalVisionFusion() fuseHeading(); } - } else if (_control_status.flags.ev_pos && (_time_last_imu - _time_last_ext_vision > (uint64_t)_params.no_gps_timeout_max)) { + + } else if (_control_status.flags.ev_pos + && (_time_last_imu - _time_last_ext_vision > (uint64_t)_params.no_gps_timeout_max)) { + // Turn off EV fusion mode if no data has been received _control_status.flags.ev_pos = false; ECL_INFO("EKF External Vision Data Stopped"); @@ -331,12 +344,15 @@ void Ekf::controlOpticalFlowFusion() bool motion_is_excessive = ((_accel_mag_filt > 10.8f) || (_accel_mag_filt < 8.8f) || (_ang_rate_mag_filt > 0.5f) - || (_R_to_earth(2,2) < 0.866f)); + || (_R_to_earth(2, 2) < 0.866f)); + if (motion_is_excessive) { _time_bad_motion_us = _imu_sample_delayed.time_us; + } else { _time_good_motion_us = _imu_sample_delayed.time_us; } + } else { _time_bad_motion_us = 0; _time_good_motion_us = _imu_sample_delayed.time_us; @@ -348,6 +364,7 @@ void Ekf::controlOpticalFlowFusion() if ((_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5) { _inhibit_gndobs_use = true; } + } else { if ((_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6) { _inhibit_gndobs_use = false; @@ -357,14 +374,14 @@ void Ekf::controlOpticalFlowFusion() // Handle cases where we are using optical flow but are no longer able to because data is old // or its use has been inhibited. if (_control_status.flags.opt_flow) { - if (_inhibit_gndobs_use) { - _control_status.flags.opt_flow = false; - _time_last_of_fuse = 0; + if (_inhibit_gndobs_use) { + _control_status.flags.opt_flow = false; + _time_last_of_fuse = 0; - } else if (_time_last_imu - _flow_sample_delayed.time_us > (uint64_t)_params.no_gps_timeout_max) { - _control_status.flags.opt_flow = false; + } else if (_time_last_imu - _flow_sample_delayed.time_us > (uint64_t)_params.no_gps_timeout_max) { + _control_status.flags.opt_flow = false; - } + } } if (_flow_data_ready) { @@ -376,10 +393,10 @@ void Ekf::controlOpticalFlowFusion() // optical flow fusion mode selection logic if ((_params.fusion_mode & MASK_USE_OF) // optical flow has been selected by the user - && !_control_status.flags.opt_flow // we are not yet using flow data - && _control_status.flags.tilt_align // we know our tilt attitude - && get_terrain_valid()) // we have a valid distance to ground estimate - { + && !_control_status.flags.opt_flow // we are not yet using flow data + && _control_status.flags.tilt_align // we know our tilt attitude + && get_terrain_valid()) { // we have a valid distance to ground estimate + // If the heading is not aligned, reset the yaw and magnetic field states if (!_control_status.flags.yaw_align) { _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); @@ -402,6 +419,7 @@ void Ekf::controlOpticalFlowFusion() } } + } else if (!(_params.fusion_mode & MASK_USE_OF)) { _control_status.flags.opt_flow = false; @@ -409,9 +427,11 @@ void Ekf::controlOpticalFlowFusion() // handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period if (_control_status.flags.opt_flow - && !_control_status.flags.gps - && !_control_status.flags.ev_pos) { + && !_control_status.flags.gps + && !_control_status.flags.ev_pos) { + bool do_reset = _time_last_imu - _time_last_of_fuse > _params.no_gps_timeout_max; + if (do_reset) { resetVelocity(); resetPosition(); @@ -441,7 +461,9 @@ void Ekf::controlGpsFusion() // Determine if we should use GPS aiding for velocity and horizontal position // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) { - if (_control_status.flags.tilt_align && _NED_origin_initialised && (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6)) { + if (_control_status.flags.tilt_align && _NED_origin_initialised + && (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6)) { + // If the heading is not aligned, reset the yaw and magnetic field states if (!_control_status.flags.yaw_align) { _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); @@ -453,15 +475,18 @@ void Ekf::controlGpsFusion() // if we are not already aiding with optical flow, then we need to reset the position and velocity // otherwise we only need to reset the position _control_status.flags.gps = true; + if (!_control_status.flags.opt_flow) { if (!resetPosition() || !resetVelocity()) { _control_status.flags.gps = false; } + } else if (!resetPosition()) { _control_status.flags.gps = false; } + if (_control_status.flags.gps) { ECL_INFO("EKF commencing GPS fusion"); _time_last_gps = _time_last_imu; @@ -493,6 +518,7 @@ void Ekf::controlGpsFusion() // if flying a fixed wing aircraft, do a complete reset that includes yaw realignYawGPS(); } + resetVelocity(); resetPosition(); _velpos_reset_request = false; @@ -512,7 +538,7 @@ void Ekf::controlGpsFusion() _fuse_hor_vel = true; // correct velocity for offset relative to IMU - Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f/_imu_sample_delayed.delta_ang_dt); + Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt); Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; Vector3f vel_offset_body = cross_product(ang_rate, pos_offset_body); Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; @@ -526,16 +552,19 @@ void Ekf::controlGpsFusion() // calculate observation process noise float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); + if (_control_status.flags.opt_flow || _control_status.flags.ev_pos) { // if we are using other sources of aiding, then relax the upper observation // noise limit which prevents bad GPS perturbing the position estimate _posObsNoiseNE = fmaxf(_gps_sample_delayed.hacc, lower_limit); + } else { // if we are not using another source of aiding, then we are reliant on the GPS // observations to constrain attitude errors and must limit the observation noise value. float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); _posObsNoiseNE = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit); } + _velObsVarNE(1) = _velObsVarNE(0) = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise)); // calculate innovations @@ -551,9 +580,8 @@ void Ekf::controlGpsFusion() } } else if (_control_status.flags.gps && (_time_last_imu - _time_last_gps > (uint64_t)10e6)) { - _control_status.flags.gps = false; - ECL_WARN("EKF GPS data stopped"); - + _control_status.flags.gps = false; + ECL_WARN("EKF GPS data stopped"); } } @@ -578,6 +606,7 @@ void Ekf::controlHeightSensorTimeouts() // record time of last bad vert accel if (bad_vert_accel) { _time_bad_vert_accel = _time_last_imu; + } else { _time_good_vert_accel = _time_last_imu; } @@ -586,6 +615,7 @@ void Ekf::controlHeightSensorTimeouts() // for a minimum of 10 seconds if (_bad_vert_accel_detected) { _bad_vert_accel_detected = (_time_last_imu - _time_bad_vert_accel < BADACC_PROBATION); + } else { _bad_vert_accel_detected = bad_vert_accel; } @@ -604,11 +634,11 @@ void Ekf::controlHeightSensorTimeouts() // handle the case where we are using baro for height if (_control_status.flags.baro_hgt) { // check if GPS height is available - const gpsSample& gps_init = _gps_buffer.get_newest(); + const gpsSample &gps_init = _gps_buffer.get_newest(); bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL); bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); - const baroSample& baro_init = _baro_buffer.get_newest(); + const baroSample &baro_init = _baro_buffer.get_newest(); bool baro_hgt_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); // check for inertial sensing errors in the last 10 seconds @@ -659,12 +689,12 @@ void Ekf::controlHeightSensorTimeouts() // handle the case we are using GPS for height if (_control_status.flags.gps_hgt) { // check if GPS height is available - const gpsSample& gps_init = _gps_buffer.get_newest(); + const gpsSample &gps_init = _gps_buffer.get_newest(); bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL); bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); // check the baro height source for consistency and freshness - const baroSample& baro_init = _baro_buffer.get_newest(); + const baroSample &baro_init = _baro_buffer.get_newest(); bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset); bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P[9][9]) * sq(_params.baro_innov_gate); @@ -711,11 +741,11 @@ void Ekf::controlHeightSensorTimeouts() // handle the case we are using range finder for height if (_control_status.flags.rng_hgt) { // check if range finder data is available - const rangeSample& rng_init = _range_buffer.get_newest(); + const rangeSample &rng_init = _range_buffer.get_newest(); bool rng_data_available = ((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL); // check if baro data is available - const baroSample& baro_init = _baro_buffer.get_newest(); + const baroSample &baro_init = _baro_buffer.get_newest(); bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); // reset to baro if we have no range data and baro data is available @@ -757,11 +787,11 @@ void Ekf::controlHeightSensorTimeouts() // handle the case where we are using external vision data for height if (_control_status.flags.ev_hgt) { // check if vision data is available - const extVisionSample& ev_init = _ext_vision_buffer.get_newest(); + const extVisionSample &ev_init = _ext_vision_buffer.get_newest(); bool ev_data_available = ((_time_last_imu - ev_init.time_us) < 2 * EV_MAX_INTERVAL); // check if baro data is available - const baroSample& baro_init = _baro_buffer.get_newest(); + const baroSample &baro_init = _baro_buffer.get_newest(); bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); // reset to baro if we have no vision data and baro data is available @@ -831,6 +861,7 @@ void Ekf::controlHeightFusion() if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { if (get_terrain_valid()) { _hgt_sensor_offset = _terrain_vpos; + } else { _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); } @@ -851,8 +882,9 @@ void Ekf::controlHeightFusion() // since takeoff. if (_control_status.flags.gnd_effect) { if ((_time_last_imu - _time_last_gnd_effect_on > GNDEFFECT_TIMEOUT) || - (((_last_on_ground_posD - _state.pos(2)) > _params.gnd_effect_max_hgt) && - _control_status.flags.in_air)) { + (((_last_on_ground_posD - _state.pos(2)) > _params.gnd_effect_max_hgt) && + _control_status.flags.in_air)) { + _control_status.flags.gnd_effect = false; } } @@ -882,14 +914,17 @@ void Ekf::controlHeightFusion() if (_control_status.flags.in_air && get_terrain_valid()) { _hgt_sensor_offset = _terrain_vpos; + } else if (_control_status.flags.in_air) { _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); + } else { _hgt_sensor_offset = _params.rng_gnd_clearance; } } + } else if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && _baro_data_ready && !_baro_hgt_faulty) { setControlBaroHeight(); _fuse_height = true; @@ -914,6 +949,7 @@ void Ekf::controlHeightFusion() if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { if (get_terrain_valid()) { _hgt_sensor_offset = _terrain_vpos; + } else { _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); } @@ -929,6 +965,7 @@ void Ekf::controlHeightFusion() if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) { _hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2); } + } else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) { // switch to baro if there was a reset to baro _fuse_height = true; @@ -968,7 +1005,9 @@ void Ekf::controlHeightFusion() _baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f); } - if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt && !_range_data_ready) { + if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt + && !_range_data_ready) { + // If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements // and are on the ground, then synthesise a measurement at the expected on ground value if (!_control_status.flags.in_air) { @@ -993,6 +1032,7 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode) if (_params.range_aid) { // check if we should use range finder measurements to estimate height, use hysteresis to avoid rapid switching bool use_range_finder; + if (in_range_aid_mode) { use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && get_terrain_valid(); @@ -1003,7 +1043,7 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode) } bool horz_vel_valid = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) - && (_fault_status.value == 0); + && (_fault_status.value == 0); if (horz_vel_valid) { float ground_vel = sqrtf(_state.vel(0) * _state.vel(0) + _state.vel(1) * _state.vel(1)); @@ -1041,7 +1081,8 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode) void Ekf::checkForStuckRange() { if (_range_data_ready && _range_sample_delayed.time_us - _time_last_rng_ready > (uint64_t)10e6 && - _control_status.flags.in_air) { + _control_status.flags.in_air) { + _control_status.flags.rng_stuck = true; //require a variance of rangefinder values to check for "stuck" measurements @@ -1118,7 +1159,8 @@ void Ekf::controlBetaFusion() // If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > (uint64_t)10e6; bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > (uint64_t)10e6; - if(_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) { + + if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) { _control_status.flags.wind = false; } @@ -1127,7 +1169,7 @@ void Ekf::controlBetaFusion() // Suffient time has lapsed sice the last fusion bool beta_fusion_time_triggered = _time_last_imu - _time_last_beta_fuse > _params.beta_avg_ft_us; - if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air) { + if (beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air) { // If starting wind state estimation, reset the wind states and covariances before fusing any data if (!_control_status.flags.wind) { // activate the wind states @@ -1158,6 +1200,7 @@ void Ekf::controlDragFusion() fuseDrag(); } + } else { _control_status.flags.wind = false; @@ -1185,6 +1228,7 @@ void Ekf::controlMagFusion() // do no magnetometer fusion at all _control_status.flags.mag_hdg = false; _control_status.flags.mag_3D = false; + } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO || _params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) { // Check if height has increased sufficiently to be away from ground magnetic anomalies bool height_achieved = (_last_on_ground_posD - _state.pos(2)) > 1.5f; @@ -1193,9 +1237,11 @@ void Ekf::controlMagFusion() // Apply hysteresis to check to avoid rapid toggling if (_yaw_angle_observable) { _yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate; + } else { _yaw_angle_observable = _accel_lpf_NE.norm() > 2.0f * _params.mag_acc_gate; } + _yaw_angle_observable = _yaw_angle_observable && (_control_status.flags.gps || _control_status.flags.ev_pos); // check if there is enough yaw rotation to make the mag bias states observable @@ -1204,6 +1250,7 @@ void Ekf::controlMagFusion() _mag_bias_observable = true; _yaw_delta_ef = 0.0f; _time_yaw_started = _imu_sample_delayed.time_us; + } else if (_mag_bias_observable) { // monitor yaw rotation in 45 deg sections. // a rotation of 45 deg is sufficient to make the mag bias observable @@ -1211,9 +1258,11 @@ void Ekf::controlMagFusion() _time_yaw_started = _imu_sample_delayed.time_us; _yaw_delta_ef = 0.0f; } + // require sustained yaw motion of 50% the initial yaw rate threshold float min_yaw_change_req = 0.5f * _params.mag_yaw_rate_gate * (1e-6f * (float)(_imu_sample_delayed.time_us - _time_yaw_started)); _mag_bias_observable = fabsf(_yaw_delta_ef) > min_yaw_change_req; + } else { _mag_bias_observable = false; } @@ -1236,15 +1285,19 @@ void Ekf::controlMagFusion() // If we are flying a vehicle that flies forward, eg plane, then we can use the GPS course to check and correct the heading if (_control_status.flags.fixed_wing && _control_status.flags.in_air) { _flt_mag_align_complete = realignYawGPS(); + if (_velpos_reset_request) { resetVelocity(); resetPosition(); _velpos_reset_request = false; } + } else { _flt_mag_align_complete = resetMagHeading(_mag_sample_delayed.mag); } + _control_status.flags.yaw_align = _control_status.flags.yaw_align || _flt_mag_align_complete; + } else { // reset the mag field covariances zeroRows(P, 16, 21); @@ -1252,7 +1305,7 @@ void Ekf::controlMagFusion() // re-instate the last used variances for (uint8_t index = 0; index <= 5; index ++) { - P[index+16][index+16] = _saved_mag_variance[index]; + P[index + 16][index + 16] = _saved_mag_variance[index]; } } } @@ -1265,10 +1318,12 @@ void Ekf::controlMagFusion() // save magnetic field state variances for next time if (_control_status.flags.mag_3D) { for (uint8_t index = 0; index <= 5; index ++) { - _saved_mag_variance[index] = P[index+16][index+16]; + _saved_mag_variance[index] = P[index + 16][index + 16]; } + _control_status.flags.mag_3D = false; } + _control_status.flags.mag_hdg = true; } @@ -1291,8 +1346,9 @@ void Ekf::controlMagFusion() // the covariance terms to allow the field states re-learn rapidly zeroRows(P, 16, 21); zeroCols(P, 16, 21); + for (uint8_t index = 0; index <= 5; index ++) { - P[index+16][index+16] = sq(_params.mag_noise); + P[index + 16][index + 16] = sq(_params.mag_noise); } } @@ -1351,10 +1407,12 @@ void Ekf::controlVelPosFusion() if (!(_params.fusion_mode & MASK_USE_GPS)) { _control_status.flags.gps = false; } + if (!_control_status.flags.gps && - !_control_status.flags.opt_flow && - !_control_status.flags.ev_pos && - !(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta)) { + !_control_status.flags.opt_flow && + !_control_status.flags.ev_pos && + !(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta)) { + // We now need to use a synthetic positon observation to prevent unconstrained drift of the INS states. _using_synthetic_position = true; @@ -1365,6 +1423,7 @@ void Ekf::controlVelPosFusion() resetPosition(); resetVelocity(); _fuse_hpos_as_odom = false; + if (_time_last_fake_gps != 0) { ECL_WARN("EKF stopping navigation"); } @@ -1378,9 +1437,11 @@ void Ekf::controlVelPosFusion() if (_control_status.flags.in_air && _control_status.flags.tilt_align) { _posObsNoiseNE = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise); + } else { _posObsNoiseNE = 0.5f; } + _vel_pos_innov[0] = 0.0f; _vel_pos_innov[1] = 0.0f; _vel_pos_innov[2] = 0.0f; @@ -1391,6 +1452,7 @@ void Ekf::controlVelPosFusion() _posInnovGateNE = 100.0f; } + } else { _using_synthetic_position = false; } diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 14b4e3cb69..47a324144c 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -72,12 +72,15 @@ void Ekf::initialiseCovariance() // position P[7][7] = sq(fmaxf(_params.gps_pos_noise, 0.01f)); P[8][8] = P[7][7]; + if (_control_status.flags.rng_hgt) { P[9][9] = sq(fmaxf(_params.range_noise, 0.01f)); + } else if (_control_status.flags.gps_hgt) { float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); P[9][9] = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit)); + } else { P[9][9] = sq(fmaxf(_params.baro_noise, 0.01f)); } @@ -165,32 +168,38 @@ void Ekf::predictCovariance() _accel_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), alpha * _accel_mag_filt); if (_ang_rate_mag_filt > _params.acc_bias_learn_gyr_lim - || _accel_mag_filt > _params.acc_bias_learn_acc_lim - || _bad_vert_accel_detected) { + || _accel_mag_filt > _params.acc_bias_learn_acc_lim + || _bad_vert_accel_detected) { + // store the bias state variances to be reinstated later if (!_accel_bias_inhibit) { _prev_dvel_bias_var(0) = P[13][13]; _prev_dvel_bias_var(1) = P[14][14]; _prev_dvel_bias_var(2) = P[15][15]; } + _accel_bias_inhibit = true; + } else { if (_accel_bias_inhibit) { // reinstate the bias state variances P[13][13] = _prev_dvel_bias_var(0); P[14][14] = _prev_dvel_bias_var(1); P[15][15] = _prev_dvel_bias_var(2); + } else { // store the bias state variances to be reinstated later _prev_dvel_bias_var(0) = P[13][13]; _prev_dvel_bias_var(1) = P[14][14]; _prev_dvel_bias_var(2) = P[15][15]; } + _accel_bias_inhibit = false; } // Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned float mag_I_sig; + if (_control_status.flags.mag_3D && (P[16][16] + P[17][17] + P[18][18]) < 0.1f) { mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f); @@ -200,6 +209,7 @@ void Ekf::predictCovariance() // Don't continue to grow the body field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned float mag_B_sig; + if (_control_status.flags.mag_3D && (P[19][19] + P[20][20] + P[21][21]) < 0.1f) { mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f); @@ -222,6 +232,7 @@ void Ekf::predictCovariance() for (unsigned i = 0; i <= 9; i++) { process_noise[i] = 0.0f; } + // delta angle bias states process_noise[12] = process_noise[11] = process_noise[10] = sq(d_ang_bias_sig); // delta_velocity bias states @@ -240,11 +251,13 @@ void Ekf::predictCovariance() float gyro_noise = math::constrain(_params.gyro_noise, 0.0f, 1.0f); daxVar = dayVar = dazVar = sq(dt * gyro_noise); float accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f); + if (_bad_vert_accel_detected) { // Increase accelerometer process noise if bad accel data is detected. Measurement errors due to // vibration induced clipping commonly reach an equivalent 0.5g offset. accel_noise = BADACC_BIAS_PNOISE; } + dvxVar = dvyVar = dvzVar = sq(dt * accel_noise); // predict the covariance @@ -467,8 +480,8 @@ void Ekf::predictCovariance() } else { // Inhibit delta velocity bias learning by zeroing the covariance terms - zeroRows(nextP,13,15); - zeroCols(nextP,13,15); + zeroRows(nextP, 13, 15); + zeroCols(nextP, 13, 15); } // Don't do covariance prediction on magnetic field states unless we are using 3-axis fusion @@ -725,15 +738,16 @@ void Ekf::fixCovarianceErrors() } // force symmetry on the quaternion, velocity and positon state covariances - makeSymmetrical(P,0,12); + makeSymmetrical(P, 0, 12); // the following states are optional and are deactivaed when not required // by ensuring the corresponding covariance matrix values are kept at zero // accelerometer bias states if ((_params.fusion_mode & MASK_INHIBIT_ACC_BIAS) || _accel_bias_inhibit) { - zeroRows(P,13,15); - zeroCols(P,13,15); + zeroRows(P, 13, 15); + zeroCols(P, 13, 15); + } else { // Find the maximum delta velocity bias state variance and request a covariance reset if any variance is below the safe minimum const float minSafeStateVar = 1e-9f; @@ -795,6 +809,7 @@ void Ekf::fixCovarianceErrors() if (!bad_acc_bias) { _fault_status.flags.bad_acc_bias = false; _time_acc_bias_check = _time_last_imu; + } else { _fault_status.flags.bad_acc_bias = true; } @@ -805,63 +820,69 @@ void Ekf::fixCovarianceErrors() float varX = P[13][13]; float varY = P[14][14]; float varZ = P[15][15]; - zeroRows(P,13,15); - zeroCols(P,13,15); + zeroRows(P, 13, 15); + zeroCols(P, 13, 15); P[13][13] = varX; P[14][14] = varY; P[15][15] = varZ; _time_acc_bias_check = _time_last_imu; _fault_status.flags.bad_acc_bias = false; ECL_WARN("EKF invalid accel bias - resetting covariance"); + } else { // ensure the covariance values are symmetrical - makeSymmetrical(P,13,15); + makeSymmetrical(P, 13, 15); } } // magnetic field states if (!_control_status.flags.mag_3D) { - zeroRows(P,16,21); - zeroCols(P,16,21); + zeroRows(P, 16, 21); + zeroCols(P, 16, 21); + } else { // constrain variances for (int i = 16; i <= 18; i++) { P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[5]); } + for (int i = 19; i <= 21; i++) { P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[6]); } + // force symmetry - makeSymmetrical(P,16,21); + makeSymmetrical(P, 16, 21); } // wind velocity states if (!_control_status.flags.wind) { - zeroRows(P,22,23); - zeroCols(P,22,23); + zeroRows(P, 22, 23); + zeroCols(P, 22, 23); + } else { // constrain variances for (int i = 22; i <= 23; i++) { P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[7]); } + // force symmetry - makeSymmetrical(P,22,23); + makeSymmetrical(P, 22, 23); } } void Ekf::resetMagCovariance() { // set the quaternion covariance terms to zero - zeroRows(P,0,3); - zeroCols(P,0,3); + zeroRows(P, 0, 3); + zeroCols(P, 0, 3); // set the magnetic field covariance terms to zero - zeroRows(P,16,21); - zeroCols(P,16,21); + zeroRows(P, 16, 21); + zeroCols(P, 16, 21); // set the field state variance to the observation variance - for (uint8_t rc_index=16; rc_index <= 21; rc_index ++) { + for (uint8_t rc_index = 16; rc_index <= 21; rc_index ++) { P[rc_index][rc_index] = sq(_params.mag_noise); } } @@ -869,15 +890,15 @@ void Ekf::resetMagCovariance() void Ekf::resetWindCovariance() { // set the wind covariance terms to zero - zeroRows(P,22,23); - zeroCols(P,22,23); + zeroRows(P, 22, 23); + zeroCols(P, 22, 23); if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) { // Use airspeed and zer sideslip assumption to set initial covariance values for wind states // calculate the wind speed and bearing - float spd = sqrtf(sq(_state.wind_vel(0))+sq(_state.wind_vel(1))); - float yaw = atan2f(_state.wind_vel(1),_state.wind_vel(0)); + float spd = sqrtf(sq(_state.wind_vel(0)) + sq(_state.wind_vel(1))); + float yaw = atan2f(_state.wind_vel(1), _state.wind_vel(0)); // calculate the uncertainty in wind speed and direction using the uncertainty in airspeed and sideslip angle // used to calculate the initial wind speed @@ -889,12 +910,12 @@ void Ekf::resetWindCovariance() float sin_yaw = sinf(yaw); float cos_yaw_2 = sq(cos_yaw); float sin_yaw_2 = sq(sin_yaw); - float sin_cos_yaw = sin_yaw*cos_yaw; + float sin_cos_yaw = sin_yaw * cos_yaw; float spd_2 = sq(spd); - P[22][22] = R_yaw*spd_2*sin_yaw_2 + R_spd*cos_yaw_2; - P[22][23] = - R_yaw*sin_cos_yaw*spd_2 + R_spd*sin_cos_yaw; + P[22][22] = R_yaw * spd_2 * sin_yaw_2 + R_spd * cos_yaw_2; + P[22][23] = - R_yaw * sin_cos_yaw * spd_2 + R_spd * sin_cos_yaw; P[23][22] = P[22][23]; - P[23][23] = R_yaw*spd_2*cos_yaw_2 + R_spd*sin_yaw_2; + P[23][23] = R_yaw * spd_2 * cos_yaw_2 + R_spd * sin_yaw_2; // Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed P[22][22] += P[4][4]; diff --git a/EKF/drag_fusion.cpp b/EKF/drag_fusion.cpp index 3e744e5f3d..cfb8bd3e24 100644 --- a/EKF/drag_fusion.cpp +++ b/EKF/drag_fusion.cpp @@ -57,6 +57,7 @@ void Ekf::fuseDrag() if (_params.bcoef_x < 1.0f || _params.bcoef_y < 1.0f) { return; } + float BC_inv_x = 1.0f / _params.bcoef_x; float BC_inv_y = 1.0f / _params.bcoef_y; @@ -149,6 +150,7 @@ void Ekf::fuseDrag() // calculate the predicted acceleration and innovation measured along the X body axis float drag_sign; + if (rel_wind(axis_index) >= 0.0f) { drag_sign = 1.0f; @@ -223,6 +225,7 @@ void Ekf::fuseDrag() // calculate the predicted acceleration and innovation measured along the Y body axis float drag_sign; + if (rel_wind(axis_index) >= 0.0f) { drag_sign = 1.0f; @@ -243,6 +246,7 @@ void Ekf::fuseDrag() // then calculate P - KHP float KHP[_k_num_states][_k_num_states]; float KH[9]; + for (unsigned row = 0; row < _k_num_states; row++) { KH[0] = Kfusion[row] * H_ACC[0]; @@ -272,12 +276,13 @@ void Ekf::fuseDrag() // if the covariance correction will result in a negative variance, then // the covariance marix is unhealthy and must be corrected bool healthy = true; + //_fault_status.flags.bad_sideslip = false; for (int i = 0; i < _k_num_states; i++) { if (P[i][i] < KHP[i][i]) { // zero rows and columns - zeroRows(P,i,i); - zeroCols(P,i,i); + zeroRows(P, i, i); + zeroCols(P, i, i); //flag as unhealthy healthy = false; diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index bedad0f263..2fa6d7810e 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -139,7 +139,7 @@ bool Ekf::initialiseFilter() // Keep accumulating measurements until we have a minimum of 10 samples for the required sensors // Sum the IMU delta angle measurements - const imuSample& imu_init = _imu_buffer.get_newest(); + const imuSample &imu_init = _imu_buffer.get_newest(); _delVel_sum += imu_init.delta_vel; // Sum the magnetometer measurements @@ -191,7 +191,8 @@ bool Ekf::initialiseFilter() // accumulate enough height measurements to be confident in the qulaity of the data if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS || - _primary_hgt_source == VDIST_SENSOR_RANGE || _primary_hgt_source == VDIST_SENSOR_EV) { + _primary_hgt_source == VDIST_SENSOR_RANGE || _primary_hgt_source == VDIST_SENSOR_EV) { + // if the user parameter specifies use of GPS/range/EV finder for height we use baro height initially and switch to GPS/range/EV finder // later when it passes checks. if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { @@ -275,14 +276,16 @@ bool Ekf::initialiseFilter() _control_status.flags.yaw_align = resetMagHeading(mag_init); // initialise the rotation from EV to EKF navigation frame if required - if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !(_params.fusion_mode & MASK_USE_EVYAW)) { + if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) + && !(_params.fusion_mode & MASK_USE_EVYAW)) { + resetExtVisRotMat(); } if (_control_status.flags.rng_hgt) { // if we are using the range finder as the primary source, then calculate the baro height at origin so we can use baro as a backup // so it can be used as a backup ad set the initial height using the range finder - const baroSample& baro_newest = _baro_buffer.get_newest(); + const baroSample &baro_newest = _baro_buffer.get_newest(); _baro_hgt_offset = baro_newest.hgt; _state.pos(2) = -math::max(_rng_filt_state * _R_rng_to_earth_2_2, _params.rng_gnd_clearance); ECL_INFO("EKF using range finder height - commencing alignment"); @@ -443,7 +446,7 @@ bool Ekf::collect_imu(imuSample &imu) void Ekf::calculateOutputStates() { // Use full rate IMU data at the current time horizon - const imuSample& imu_new = _imu_sample_new; + const imuSample &imu_new = _imu_sample_new; // correct delta angles for bias offsets Vector3f delta_angle; @@ -453,9 +456,9 @@ void Ekf::calculateOutputStates() delta_angle(2) = _imu_sample_new.delta_ang(2) - _state.gyro_bias(2) * dt_scale_correction; // calculate a yaw change about the earth frame vertical - float spin_del_ang_D = _R_to_earth_now(2,0) * delta_angle(0) + - _R_to_earth_now(2,1) * delta_angle(1) + - _R_to_earth_now(2,2) * delta_angle(2); + float spin_del_ang_D = _R_to_earth_now(2, 0) * delta_angle(0) + + _R_to_earth_now(2, 1) * delta_angle(1) + + _R_to_earth_now(2, 2) * delta_angle(2); _yaw_delta_ef += spin_del_ang_D; // Calculate filtered yaw rate to be used by the magnetomer fusion type selection logic @@ -615,13 +618,14 @@ void Ekf::calculateOutputStates() for (uint8_t counter = 0; counter < (size - 1); counter++) { const uint8_t index_next = (index + 1) % size; - outputVert& current_state = _output_vert_buffer[index]; - outputVert& next_state = _output_vert_buffer[index_next]; + outputVert ¤t_state = _output_vert_buffer[index]; + outputVert &next_state = _output_vert_buffer[index_next]; // correct the velocity if (counter == 0) { current_state.vel_d += vel_d_correction; } + next_state.vel_d += vel_d_correction; // position is propagated forward using the corrected velocity and a trapezoidal integrator diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 7f79ef02e7..70cb873582 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -58,7 +58,7 @@ bool Ekf::resetVelocity() _state.vel = _gps_sample_delayed.vel; // use GPS accuracy to reset variances - setDiag(P,4,6,sq(_gps_sample_delayed.sacc)); + setDiag(P, 4, 6, sq(_gps_sample_delayed.sacc)); } else if (_control_status.flags.opt_flow) { // constrain height above ground to be above minimum possible @@ -89,21 +89,21 @@ bool Ekf::resetVelocity() } // reset the velocity covariance terms - zeroRows(P,4,5); - zeroCols(P,4,5); + zeroRows(P, 4, 5); + zeroCols(P, 4, 5); // reset the horizontal velocity variance using the optical flow noise variance P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar(); } else if (_control_status.flags.ev_pos) { _state.vel.setZero(); - zeroOffDiag(P,4,6); + zeroOffDiag(P, 4, 6); } else { // Used when falling back to non-aiding mode of operation _state.vel(0) = 0.0f; _state.vel(1) = 0.0f; - setDiag(P,4,5,25.0f); + setDiag(P, 4, 5, 25.0f); } // calculate the change in velocity and apply to the output predictor state history @@ -145,7 +145,7 @@ bool Ekf::resetPosition() _state.pos(1) = _gps_sample_delayed.pos(1); // use GPS accuracy to reset variances - setDiag(P,7,8,sq(_gps_sample_delayed.hacc)); + setDiag(P, 7, 8, sq(_gps_sample_delayed.hacc)); } else if (_control_status.flags.ev_pos) { // this reset is only called if we have new ev data at the fusion time horizon @@ -153,7 +153,7 @@ bool Ekf::resetPosition() _state.pos(1) = _ev_sample_delayed.posNED(1); // use EV accuracy to reset variances - setDiag(P,7,8,sq(_ev_sample_delayed.posErr)); + setDiag(P, 7, 8, sq(_ev_sample_delayed.posErr)); } else if (_control_status.flags.opt_flow) { if (!_control_status.flags.in_air) { @@ -167,13 +167,14 @@ bool Ekf::resetPosition() _state.pos(1) = _last_known_posNE(1); } - setDiag(P,7,8,sq(_params.pos_noaid_noise)); + + setDiag(P, 7, 8, sq(_params.pos_noaid_noise)); } else { // Used when falling back to non-aiding mode of operation _state.pos(0) = _last_known_posNE(0); _state.pos(1) = _last_known_posNE(1); - setDiag(P,7,8,sq(_params.pos_noaid_noise)); + setDiag(P, 7, 8, sq(_params.pos_noaid_noise)); } // calculate the change in position and apply to the output predictor state history @@ -200,7 +201,7 @@ bool Ekf::resetPosition() void Ekf::resetHeight() { // Get the most recent GPS data - const gpsSample& gps_newest = _gps_buffer.get_newest(); + const gpsSample &gps_newest = _gps_buffer.get_newest(); // store the current vertical position and velocity for reference so we can calculate and publish the reset amount float old_vert_pos = _state.pos(2); @@ -233,7 +234,7 @@ void Ekf::resetHeight() vert_pos_reset = true; // reset the baro offset which is subtracted from the baro reading if we need to use it as a backup - const baroSample& baro_newest = _baro_buffer.get_newest(); + const baroSample &baro_newest = _baro_buffer.get_newest(); _baro_hgt_offset = baro_newest.hgt + _state.pos(2); } else { @@ -243,7 +244,7 @@ void Ekf::resetHeight() } else if (_control_status.flags.baro_hgt) { // initialize vertical position with newest baro measurement - const baroSample& baro_newest = _baro_buffer.get_newest(); + const baroSample &baro_newest = _baro_buffer.get_newest(); if (_time_last_imu - baro_newest.time_us < 2 * BARO_MAX_INTERVAL) { _state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset; @@ -276,7 +277,7 @@ void Ekf::resetHeight() vert_pos_reset = true; // reset the baro offset which is subtracted from the baro reading if we need to use it as a backup - const baroSample& baro_newest = _baro_buffer.get_newest(); + const baroSample &baro_newest = _baro_buffer.get_newest(); _baro_hgt_offset = baro_newest.hgt + _state.pos(2); } else { @@ -285,7 +286,7 @@ void Ekf::resetHeight() } else if (_control_status.flags.ev_hgt) { // initialize vertical position with newest measurement - const extVisionSample& ev_newest = _ext_vision_buffer.get_newest(); + const extVisionSample &ev_newest = _ext_vision_buffer.get_newest(); // use the most recent data if it's time offset from the fusion time horizon is smaller int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us; @@ -365,6 +366,7 @@ void Ekf::resetHeight() _output_vert_delayed.vel_d_integ = _state.pos(2); _output_vert_new.vel_d_integ = _state.pos(2); } + if (vert_vel_reset) { _output_vert_delayed.vel_d = _state.vel(2); _output_vert_new.vel_d = _state.vel(2); @@ -398,6 +400,7 @@ bool Ekf::realignYawGPS() { // Need at least 5 m/s of GPS horizontal speed and ratio of velocity error to velocity < 0.15 for a reliable alignment float gpsSpeed = sqrtf(sq(_gps_sample_delayed.vel(0)) + sq(_gps_sample_delayed.vel(1))); + if ((gpsSpeed > 5.0f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed))) { // check for excessive GPS velocity innovations bool badVelInnov = ((_vel_pos_test_ratio[0] > 1.0f) || (_vel_pos_test_ratio[1] > 1.0f)) && _control_status.flags.gps; @@ -452,11 +455,12 @@ bool Ekf::realignYawGPS() } else if (_control_status.flags.wind) { // we have previously aligned yaw in-flight and have wind estimates so set the yaw such that the vehicle nose is // aligned with the wind relative GPS velocity vector - euler321(2) = atan2f((_gps_sample_delayed.vel(1) - _state.wind_vel(1)) , (_gps_sample_delayed.vel(0) - _state.wind_vel(0))); + euler321(2) = atan2f((_gps_sample_delayed.vel(1) - _state.wind_vel(1)), + (_gps_sample_delayed.vel(0) - _state.wind_vel(0))); } else { // we don't have wind estimates, so align yaw to the GPS velocity vector - euler321(2) = atan2f(_gps_sample_delayed.vel(1) , _gps_sample_delayed.vel(0)); + euler321(2) = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0)); } @@ -699,7 +703,9 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) _R_to_earth = quat_to_invrotmat(_state.quat_nominal); // reset the rotation from the EV to EKF frame of reference if it is being used - if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !(_params.fusion_mode & MASK_USE_EVYAW)) { + if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) + && !(_params.fusion_mode & MASK_USE_EVYAW)) { + resetExtVisRotMat(); } @@ -741,6 +747,7 @@ void Ekf::calcMagDeclination() if (_flt_mag_align_complete) { // Use value consistent with earth field state _mag_declination = atan2f(_state.mag_I(1), _state.mag_I(0)); + } else if (_params.mag_declination_source & MASK_USE_GEO_DECL) { // use parameter value until GPS is available, then use value returned by geo library if (_NED_origin_initialised) { @@ -1055,10 +1062,11 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl) // If relying on optical flow for navigation we need to keep within flow and range sensor limits bool relying_on_optical_flow = _control_status.flags.opt_flow && !(_control_status.flags.gps || _control_status.flags.ev_pos); + if (relying_on_optical_flow) { // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion flow_gnd_spd_max = 0.5f * _params.flow_rate_max * (_terrain_vpos - _state.pos(2)); - flow_gnd_spd_max = fmaxf(flow_gnd_spd_max , 0.0f); + flow_gnd_spd_max = fmaxf(flow_gnd_spd_max, 0.0f); flow_limit_hagl = true; @@ -1136,7 +1144,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0); soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0); soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0); - soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow ) && (_fault_status.value == 0); + soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0); soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0); soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert; soln_status.flags.pos_vert_agl = get_terrain_valid(); @@ -1214,6 +1222,7 @@ void Ekf::zeroOffDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t fi // save diagonal elements uint8_t row; float variances[_k_num_states]; + for (row = first; row <= last; row++) { variances[row] = cov_mat[row][row]; } @@ -1236,6 +1245,7 @@ void Ekf::setDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, // set diagonals uint8_t row; + for (row = first; row <= last; row++) { cov_mat[row][row] = variance; } @@ -1268,7 +1278,7 @@ void Ekf::update_deadreckoning_status() } // report if we have been deadreckoning for too long - _deadreckon_time_exceeded = ((_time_last_imu - _time_ins_deadreckon_start) > (unsigned)_params.valid_timeout_max); + _deadreckon_time_exceeded = ((_time_last_imu - _time_ins_deadreckon_start) > (unsigned)_params.valid_timeout_max); } // perform a vector cross product @@ -1426,8 +1436,8 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var) float t44 = t17-t36; // zero all the quaternion covariances - zeroRows(P,0,3); - zeroCols(P,0,3); + zeroRows(P, 0, 3); + zeroCols(P, 0, 3); // Update the quaternion internal covariances using auto-code generated using matlab symbolic toolbox P[0][0] = rot_vec_var(0)*t2*t9*t10*0.25f+rot_vec_var(1)*t4*t9*t10*0.25f+rot_vec_var(2)*t5*t9*t10*0.25f; @@ -1518,18 +1528,20 @@ void Ekf::calcExtVisRotMat() Vector3f rot_vec = q_error.to_axis_angle(); float rot_vec_norm = rot_vec.norm(); + if (rot_vec_norm > 1e-6f) { // apply an input limiter to protect from spikes Vector3f _input_delta_vec = rot_vec - _ev_rot_vec_filt; float input_delta_mag = _input_delta_vec.norm(); + if (input_delta_mag > 0.1f) { rot_vec = _ev_rot_vec_filt + _input_delta_vec * (0.1f / input_delta_mag); } // Apply a first order IIR low pass filter const float omega_lpf_us = 0.2e-6f; // cutoff frequency in rad/uSec - float alpha = math::constrain(omega_lpf_us * (float)(_time_last_imu - _ev_rot_last_time_us) , 0.0f , 1.0f); + float alpha = math::constrain(omega_lpf_us * (float)(_time_last_imu - _ev_rot_last_time_us), 0.0f, 1.0f); _ev_rot_last_time_us = _time_last_imu; _ev_rot_vec_filt = _ev_rot_vec_filt * (1.0f - alpha) + rot_vec * alpha; @@ -1554,8 +1566,10 @@ void Ekf::resetExtVisRotMat() Vector3f rot_vec = q_error.to_axis_angle(); float rot_vec_norm = rot_vec.norm(); + if (rot_vec_norm > 1e-9f) { _ev_rot_vec_filt = rot_vec; + } else { _ev_rot_vec_filt.zero(); } @@ -1569,6 +1583,7 @@ void Ekf::get_ekf2ev_quaternion(float *quat) { Quatf quat_ekf2ev; quat_ekf2ev.from_axis_angle(_ev_rot_vec_filt); + for (unsigned i = 0; i < 4; i++) { quat[i] = quat_ekf2ev(i); } diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index b79d79e179..a4f4868ec0 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -108,6 +108,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u // Do not retry if allocation has failed previously if (_drag_buffer.get_length() < _obs_buffer_length) { _drag_buffer_fail = !_drag_buffer.allocate(_obs_buffer_length); + if (_drag_buffer_fail) { ECL_ERR("EKF drag buffer allocation failed"); return; @@ -163,6 +164,7 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3]) // Do not retry if allocation has failed previously if (_mag_buffer.get_length() < _obs_buffer_length) { _mag_buffer_fail = !_mag_buffer.allocate(_obs_buffer_length); + if (_mag_buffer_fail) { ECL_ERR("EKF mag buffer allocation failed"); return; @@ -194,6 +196,7 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps) // Do not retry if allocation has failed previously if (_gps_buffer.get_length() < _obs_buffer_length) { _gps_buffer_fail = !_gps_buffer.allocate(_obs_buffer_length); + if (_gps_buffer_fail) { ECL_ERR("EKF GPS buffer allocation failed"); return; @@ -247,6 +250,7 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float data) // Do not retry if allocation has failed previously if (_baro_buffer.get_length() < _obs_buffer_length) { _baro_buffer_fail = !_baro_buffer.allocate(_obs_buffer_length); + if (_baro_buffer_fail) { ECL_ERR("EKF baro buffer allocation failed"); return; @@ -279,6 +283,7 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float true_airspeed // Do not retry if allocation has failed previously if (_airspeed_buffer.get_length() < _obs_buffer_length) { _airspeed_buffer_fail = !_airspeed_buffer.allocate(_obs_buffer_length); + if (_airspeed_buffer_fail) { ECL_ERR("EKF airspeed buffer allocation failed"); return; @@ -308,6 +313,7 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float data) // Do not retry if allocation has failed previously if (_range_buffer.get_length() < _obs_buffer_length) { _range_buffer_fail = !_range_buffer.allocate(_obs_buffer_length); + if (_range_buffer_fail) { ECL_ERR("EKF range finder buffer allocation failed"); return; @@ -336,6 +342,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl // Do not retry if allocation has failed previously if (_flow_buffer.get_length() < _obs_buffer_length) { _flow_buffer_fail = !_flow_buffer.allocate(_obs_buffer_length); + if (_flow_buffer_fail) { ECL_ERR("EKF optical flow buffer allocation failed"); return; @@ -349,6 +356,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl float delta_time = 1e-6f * (float)flow->dt; float delta_time_min = 5e-7f * (float)_min_obs_interval_us; bool delta_time_good = delta_time >= delta_time_min; + if (!delta_time_good) { // protect against overflow casued by division with very small delta_time delta_time = delta_time_min; @@ -415,6 +423,7 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message // Do not retry if allocation has failed previously if (_ext_vision_buffer.get_length() < _obs_buffer_length) { _ev_buffer_fail = !_ext_vision_buffer.allocate(_obs_buffer_length); + if (_ev_buffer_fail) { ECL_ERR("EKF external vision buffer allocation failed"); return; @@ -451,6 +460,7 @@ void EstimatorInterface::setAuxVelData(uint64_t time_usec, float (&data)[2], flo // Do not retry if allocation has failed previously if (_auxvel_buffer.get_length() < _obs_buffer_length) { _auxvel_buffer_fail = !_auxvel_buffer.allocate(_obs_buffer_length); + if (_auxvel_buffer_fail) { ECL_ERR("EKF aux vel buffer allocation failed"); return; @@ -553,7 +563,8 @@ bool EstimatorInterface::local_position_is_valid() return !_deadreckon_time_exceeded; } -void EstimatorInterface::print_status() { +void EstimatorInterface::print_status() +{ ECL_INFO("local position valid: %s", local_position_is_valid() ? "yes" : "no"); ECL_INFO("global position valid: %s", global_position_is_valid() ? "yes" : "no"); diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 06f6f3051e..3543ff9144 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -262,6 +262,7 @@ public: void get_velocity(float *vel) { Vector3f vel_earth = _output_new.vel - _vel_imu_rel_body_ned; + for (unsigned i = 0; i < 3; i++) { vel[i] = vel_earth(i); } diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 015393d22b..d10007c248 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -62,6 +62,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) // Also run checks if the vehicle is on-ground as the check data can be used by vehicle pre-flight checks if (!_control_status.flags.in_air || !_NED_origin_initialised || !_control_status.flags.gps) { bool gps_checks_pass = gps_is_good(gps); + if (!_NED_origin_initialised && gps_checks_pass) { // If we have good GPS data set the origin's WGS-84 position to the last gps fix double lat = gps->lat / 1.0e7; @@ -93,6 +94,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) _control_status.flags.rng_hgt = false; // zero the sensor offset _hgt_sensor_offset = 0.0f; + } else { ECL_INFO("EKF GPS checks passed (WGS-84 origin set)"); } @@ -139,6 +141,7 @@ bool Ekf::gps_is_good(struct gps_message *gps) // calculate position movement since last GPS fix if (_gps_pos_prev.timestamp > 0) { map_projection_project(&_gps_pos_prev, lat, lon, &delta_posN, &delta_PosE); + } else { // no previous position has been set map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu); diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 7606a47e3c..dd3c60724b 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -146,11 +146,14 @@ void Ekf::fuseMag() // Perform an innovation consistency check and report the result bool healthy = true; + for (uint8_t index = 0; index <= 2; index++) { _mag_test_ratio[index] = sq(_mag_innov[index]) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var[index]); + if (_mag_test_ratio[index] > 1.0f) { healthy = false; _innov_check_fail_status.value |= (1 << (index + 3)); + } else { _innov_check_fail_status.value &= ~(1 << (index + 3)); } @@ -213,6 +216,7 @@ void Ekf::fuseMag() for (uint8_t i = 0; i < 16; i++) { Kfusion[i] = 0.0f; } + Kfusion[22] = 0.0f; Kfusion[23] = 0.0f; } @@ -267,6 +271,7 @@ void Ekf::fuseMag() for (uint8_t i = 0; i < 16; i++) { Kfusion[i] = 0.0f; } + Kfusion[22] = 0.0f; Kfusion[23] = 0.0f; } @@ -321,6 +326,7 @@ void Ekf::fuseMag() for (uint8_t i = 0; i < 16; i++) { Kfusion[i] = 0.0f; } + Kfusion[22] = 0.0f; Kfusion[23] = 0.0f; } @@ -341,6 +347,7 @@ void Ekf::fuseMag() // then calculate P - KHP float KHP[_k_num_states][_k_num_states]; float KH[10]; + for (unsigned row = 0; row < _k_num_states; row++) { KH[0] = Kfusion[row] * H_MAG[0]; @@ -374,11 +381,12 @@ void Ekf::fuseMag() _fault_status.flags.bad_mag_x = false; _fault_status.flags.bad_mag_y = false; _fault_status.flags.bad_mag_z = false; + for (int i = 0; i < _k_num_states; i++) { if (P[i][i] < KHP[i][i]) { // zero rows and columns - zeroRows(P,i,i); - zeroCols(P,i,i); + zeroRows(P, i, i); + zeroCols(P, i, i); //flag as unhealthy healthy = false; @@ -386,8 +394,10 @@ void Ekf::fuseMag() // update individual measurement health status if (index == 0) { _fault_status.flags.bad_mag_x = true; + } else if (index == 1) { _fault_status.flags.bad_mag_y = true; + } else if (index == 2) { _fault_status.flags.bad_mag_z = true; } @@ -473,6 +483,7 @@ void Ekf::fuseHeading() if (_control_status.flags.mag_3D) { // don't apply bias corrections if we are learning them mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + } else { mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B); } @@ -512,6 +523,7 @@ void Ekf::fuseHeading() float t12 = t8*t11*4.0f; float t13 = t12+1.0f; float t14; + if (fabsf(t13) > 1e-6f) { t14 = 1.0f/t13; } else { @@ -590,9 +602,11 @@ void Ekf::fuseHeading() if (_control_status.flags.mag_hdg) { // using magnetic heading tuning parameter R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)); + } else if (_control_status.flags.ev_yaw) { // using error estimate from external vision data R_YAW = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)); + } else { // there is no yaw observation return; @@ -602,6 +616,7 @@ void Ekf::fuseHeading() // calculate the innovaton variance float PH[4]; _heading_innov_var = R_YAW; + for (unsigned row = 0; row <= 3; row++) { PH[row] = 0.0f; @@ -696,6 +711,7 @@ void Ekf::fuseHeading() // then calculate P - KHP float KHP[_k_num_states][_k_num_states]; float KH[4]; + for (unsigned row = 0; row < _k_num_states; row++) { KH[0] = Kfusion[row] * H_YAW[0]; @@ -716,11 +732,12 @@ void Ekf::fuseHeading() // the covariance marix is unhealthy and must be corrected bool healthy = true; _fault_status.flags.bad_mag_hdg = false; + for (int i = 0; i < _k_num_states; i++) { if (P[i][i] < KHP[i][i]) { // zero rows and columns - zeroRows(P,i,i); - zeroCols(P,i,i); + zeroRows(P, i, i); + zeroCols(P, i, i); //flag as unhealthy healthy = false; @@ -853,8 +870,8 @@ void Ekf::fuseDeclination() for (int i = 0; i < _k_num_states; i++) { if (P[i][i] < KHP[i][i]) { // zero rows and columns - zeroRows(P,i,i); - zeroCols(P,i,i); + zeroRows(P, i, i); + zeroCols(P, i, i); //flag as unhealthy healthy = false; diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index eaae6a91d3..e381b4d765 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -213,6 +213,7 @@ void Ekf::fuseOptFlow() float t92 = t2*t7*t69; float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92; float t78; + // calculate innovation variance for X axis observation and protect against a badly conditioned calculation if (t77 >= R_LOS) { t78 = 1.0f / t77; @@ -403,6 +404,7 @@ void Ekf::fuseOptFlow() // record the innovation test pass/fail bool flow_fail = false; + for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) { if (optflow_test_ratio[obs_index] > 1.0f) { flow_fail = true; @@ -434,6 +436,7 @@ void Ekf::fuseOptFlow() // then calculate P - KHP float KHP[_k_num_states][_k_num_states]; float KH[7]; + for (unsigned row = 0; row < _k_num_states; row++) { KH[0] = gain[row] * H_LOS[obs_index][0]; @@ -461,11 +464,12 @@ void Ekf::fuseOptFlow() bool healthy = true; _fault_status.flags.bad_optflow_X = false; _fault_status.flags.bad_optflow_Y = false; + for (int i = 0; i < _k_num_states; i++) { if (P[i][i] < KHP[i][i]) { // zero rows and columns - zeroRows(P,i,i); - zeroCols(P,i,i); + zeroRows(P, i, i); + zeroCols(P, i, i); //flag as unhealthy healthy = false; @@ -473,6 +477,7 @@ void Ekf::fuseOptFlow() // update individual measurement health status if (obs_index == 0) { _fault_status.flags.bad_optflow_X = true; + } else if (obs_index == 1) { _fault_status.flags.bad_optflow_Y = true; } diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index a119266494..c36721d156 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -46,7 +46,7 @@ bool Ekf::initHagl() { // get most recent range measurement from buffer - const rangeSample& latest_measurement = _range_buffer.get_newest(); + const rangeSample &latest_measurement = _range_buffer.get_newest(); if ((_time_last_imu - latest_measurement.time_us) < (uint64_t)2e5 && _R_rng_to_earth_2_2 > _params.range_cos_max_tilt) { // if we have a fresh measurement, use it to initialise the terrain estimator @@ -154,6 +154,7 @@ void Ekf::fuseHagl() if (_time_last_imu - _time_last_hagl_fuse > (uint64_t)10E6) { _terrain_vpos = _state.pos(2) + meas_hagl; _terrain_var = obs_variance; + } else { _innov_check_fail_status.flags.reject_hagl = true; } @@ -170,15 +171,17 @@ void Ekf::fuseHagl() // return true if the terrain height estimate is valid bool Ekf::get_terrain_valid() { - return _hagl_valid; + return _hagl_valid; } // determine terrain validity void Ekf::update_terrain_valid() { if (_terrain_initialised && _range_data_continuous && !_control_status.flags.rng_stuck && - (_time_last_imu - _time_last_hagl_fuse < (uint64_t)5e6)) { + (_time_last_imu - _time_last_hagl_fuse < (uint64_t)5e6)) { + _hagl_valid = true; + } else { _hagl_valid = false; } diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index c7ac014604..1b9b2f1204 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -109,10 +109,12 @@ void Ekf::fuseVelPosHeight() // casued by rotor wash ground interaction by applying a temporary deadzone to baro innovations. float deadzone_start = 0.25f * _params.baro_noise; float deadzone_end = deadzone_start + _params.gnd_effect_deadzone; + if (_control_status.flags.gnd_effect) { if (innovation[5] < -deadzone_start) { if (innovation[5] <= -deadzone_end) { innovation[5] += deadzone_end; + } else { innovation[5] = -deadzone_start; } @@ -136,11 +138,12 @@ void Ekf::fuseVelPosHeight() fuse_map[5] = true; // use range finder with tilt correction innovation[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2, - _params.rng_gnd_clearance)) - _hgt_sensor_offset; + _params.rng_gnd_clearance)) - _hgt_sensor_offset; // observation variance - user parameter defined R[5] = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * sq(_R_rng_to_earth_2_2), 0.01f); // innovation gate size gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f); + } else if (_control_status.flags.ev_hgt) { fuse_map[5] = true; // calculate the innovation assuming the external vision observaton is in local NED frame @@ -183,31 +186,39 @@ void Ekf::fuseVelPosHeight() if ((_fuse_hor_vel || _fuse_hor_vel_aux) && vel_check_pass) { _time_last_vel_fuse = _time_last_imu; _innov_check_fail_status.flags.reject_vel_NED = false; + } else if (!vel_check_pass) { _innov_check_fail_status.flags.reject_vel_NED = true; } + _fuse_hor_vel = _fuse_hor_vel_aux = false; // record the successful position fusion event if (pos_check_pass && _fuse_pos) { if (!_fuse_hpos_as_odom) { _time_last_pos_fuse = _time_last_imu; + } else { _time_last_delpos_fuse = _time_last_imu; } + _innov_check_fail_status.flags.reject_pos_NE = false; + } else if (!pos_check_pass) { _innov_check_fail_status.flags.reject_pos_NE = true; } + _fuse_pos = false; // record the successful height fusion event if (innov_check_pass_map[5] && _fuse_height) { _time_last_hgt_fuse = _time_last_imu; _innov_check_fail_status.flags.reject_pos_D = false; + } else if (!innov_check_pass_map[5]) { _innov_check_fail_status.flags.reject_pos_D = true; } + _fuse_height = false; for (unsigned obs_index = 0; obs_index < 6; obs_index++) { @@ -225,6 +236,7 @@ void Ekf::fuseVelPosHeight() // update covarinace matrix via Pnew = (I - KH)P float KHP[_k_num_states][_k_num_states]; + for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned column = 0; column < _k_num_states; column++) { KHP[row][column] = Kfusion[row] * P[state_index][column]; @@ -234,11 +246,12 @@ void Ekf::fuseVelPosHeight() // if the covariance correction will result in a negative variance, then // the covariance marix is unhealthy and must be corrected bool healthy = true; + for (int i = 0; i < _k_num_states; i++) { if (P[i][i] < KHP[i][i]) { // zero rows and columns - zeroRows(P,i,i); - zeroCols(P,i,i); + zeroRows(P, i, i); + zeroCols(P, i, i); //flag as unhealthy healthy = false; @@ -246,29 +259,40 @@ void Ekf::fuseVelPosHeight() // update individual measurement health status if (obs_index == 0) { _fault_status.flags.bad_vel_N = true; + } else if (obs_index == 1) { _fault_status.flags.bad_vel_E = true; + } else if (obs_index == 2) { _fault_status.flags.bad_vel_D = true; + } else if (obs_index == 3) { _fault_status.flags.bad_pos_N = true; + } else if (obs_index == 4) { _fault_status.flags.bad_pos_E = true; + } else if (obs_index == 5) { _fault_status.flags.bad_pos_D = true; } + } else { // update individual measurement health status if (obs_index == 0) { _fault_status.flags.bad_vel_N = false; + } else if (obs_index == 1) { _fault_status.flags.bad_vel_E = false; + } else if (obs_index == 2) { _fault_status.flags.bad_vel_D = false; + } else if (obs_index == 3) { _fault_status.flags.bad_pos_N = false; + } else if (obs_index == 4) { _fault_status.flags.bad_pos_E = false; + } else if (obs_index == 5) { _fault_status.flags.bad_pos_D = false; } diff --git a/airdata/WindEstimator.cpp b/airdata/WindEstimator.cpp index de8963263d..07792b915b 100644 --- a/airdata/WindEstimator.cpp +++ b/airdata/WindEstimator.cpp @@ -339,8 +339,8 @@ WindEstimator::run_sanity_checks() } bool -WindEstimator::check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size, uint64_t &time_meas_rejected, - bool &reinit_filter) +WindEstimator::check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size, + uint64_t &time_meas_rejected, bool &reinit_filter) { if (innov * innov > gate_size * gate_size * innov_var) { time_meas_rejected = time_meas_rejected == 0 ? time_now : time_meas_rejected; diff --git a/airdata/WindEstimator.hpp b/airdata/WindEstimator.hpp index 1c712fb3d4..dee8dc4924 100644 --- a/airdata/WindEstimator.hpp +++ b/airdata/WindEstimator.hpp @@ -68,8 +68,8 @@ public: bool is_estimate_valid() { return _initialised; } - bool check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size, uint64_t &time_meas_rejected, - bool &reinit_filter); + bool check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size, + uint64_t &time_meas_rejected, bool &reinit_filter); float get_tas_scale() { return _state(tas); } float get_tas_innov() { return _tas_innov; } diff --git a/attitude_fw/ecl_roll_controller.cpp b/attitude_fw/ecl_roll_controller.cpp index dd2d2a9c5c..259a3e916c 100644 --- a/attitude_fw/ecl_roll_controller.cpp +++ b/attitude_fw/ecl_roll_controller.cpp @@ -126,7 +126,7 @@ float ECL_RollController::control_euler_rate(const struct ECL_ControlData &ctl_d _bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint; set_bodyrate_setpoint(_bodyrate_setpoint); - + return control_bodyrate(ctl_data); } diff --git a/attitude_fw/ecl_yaw_controller.cpp b/attitude_fw/ecl_yaw_controller.cpp index 7c8bc83557..d7adecb162 100644 --- a/attitude_fw/ecl_yaw_controller.cpp +++ b/attitude_fw/ecl_yaw_controller.cpp @@ -110,8 +110,8 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control if (!inverted) { /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ - _rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / (ctl_data.airspeed < ctl_data.airspeed_min ? - ctl_data.airspeed_min : ctl_data.airspeed); + _rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / (ctl_data.airspeed < + ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed); } if (!ISFINITE(_rate_setpoint)) { diff --git a/validation/data_validator.cpp b/validation/data_validator.cpp index eb56260262..c8beaf6384 100644 --- a/validation/data_validator.cpp +++ b/validation/data_validator.cpp @@ -78,6 +78,7 @@ DataValidator::put(uint64_t timestamp, float val[dimensions], uint64_t error_cou if (error_count_in > _error_count) { _error_density += (error_count_in - _error_count); + } else if (_error_density > 0) { _error_density--; } @@ -90,6 +91,7 @@ DataValidator::put(uint64_t timestamp, float val[dimensions], uint64_t error_cou _mean[i] = 0; _lp[i] = val[i]; _M2[i] = 0; + } else { float lp_val = val[i] - _lp[i]; @@ -100,6 +102,7 @@ DataValidator::put(uint64_t timestamp, float val[dimensions], uint64_t error_cou if (fabsf(_value[i] - val[i]) < 0.000001f) { _value_equal_count++; + } else { _value_equal_count = 0; } @@ -126,23 +129,23 @@ DataValidator::confidence(uint64_t timestamp) _error_mask |= ERROR_FLAG_NO_DATA; ret = 0.0f; - /* timed out - that's it */ } else if (timestamp - _time_last > _timeout_interval) { + /* timed out - that's it */ _error_mask |= ERROR_FLAG_TIMEOUT; ret = 0.0f; - /* we got the exact same sensor value N times in a row */ } else if (_value_equal_count > _value_equal_count_threshold) { + /* we got the exact same sensor value N times in a row */ _error_mask |= ERROR_FLAG_STALE_DATA; ret = 0.0f; - /* check error count limit */ } else if (_error_count > NORETURN_ERRCOUNT) { + /* check error count limit */ _error_mask |= ERROR_FLAG_HIGH_ERRCOUNT; ret = 0.0f; - /* cap error density counter at window size */ } else if (_error_density > ERROR_DENSITY_WINDOW) { + /* cap error density counter at window size */ _error_mask |= ERROR_FLAG_HIGH_ERRDENSITY; _error_density = ERROR_DENSITY_WINDOW; @@ -171,7 +174,7 @@ DataValidator::print() for (unsigned i = 0; i < dimensions; i++) { ECL_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f conf: %8.4f", - (double) _value[i], (double)_lp[i], (double)_mean[i], - (double)_rms[i], (double)confidence(ecl_absolute_time())); + (double) _value[i], (double)_lp[i], (double)_mean[i], + (double)_rms[i], (double)confidence(ecl_absolute_time())); } } diff --git a/validation/data_validator.h b/validation/data_validator.h index 6afd49b486..8d6a489756 100644 --- a/validation/data_validator.h +++ b/validation/data_validator.h @@ -44,7 +44,8 @@ #include #include -class DataValidator { +class DataValidator +{ public: static const unsigned dimensions = 3; @@ -70,13 +71,13 @@ public: * * @return the next sibling */ - DataValidator* sibling() { return _sibling; } + DataValidator *sibling() { return _sibling; } /** * Set the sibling to the next node in the group * */ - void setSibling(DataValidator* new_sibling) { _sibling = new_sibling; } + void setSibling(DataValidator *new_sibling) { _sibling = new_sibling; } /** * Get the confidence of this validator @@ -94,7 +95,7 @@ public: * Get the values of this validator * @return the stored value */ - float* value() { return _value; } + float *value() { return _value; } /** * Get the used status of this validator @@ -123,13 +124,13 @@ public: * Get the RMS values of this validator * @return the stored RMS */ - float* rms() { return _rms; } + float *rms() { return _rms; } /** * Get the vibration offset * @return the stored vibration offset */ - float* vibration_offset() { return _vibe; } + float *vibration_offset() { return _vibe; } /** * Print the validator value @@ -190,6 +191,6 @@ private: static const constexpr unsigned VALUE_EQUAL_COUNT_DEFAULT = 100; /**< if the sensor value is the same (accumulated also between axes) this many times, flag it */ /* we don't want this class to be copied */ - DataValidator(const DataValidator&) = delete; - DataValidator operator=(const DataValidator&) = delete; + DataValidator(const DataValidator &) = delete; + DataValidator operator=(const DataValidator &) = delete; }; diff --git a/validation/data_validator_group.cpp b/validation/data_validator_group.cpp index d84db5d56a..0df70485f3 100644 --- a/validation/data_validator_group.cpp +++ b/validation/data_validator_group.cpp @@ -56,17 +56,20 @@ DataValidatorGroup::DataValidatorGroup(unsigned siblings) : for (unsigned i = 0; i < siblings; i++) { next = new DataValidator(); - if(i == 0) { + + if (i == 0) { _first = next; + } else { prev->setSibling(next); } + prev = next; } _last = next; - if(_first) { + if (_first) { _timeout_interval_us = _first->get_timeout(); } } @@ -74,7 +77,7 @@ DataValidatorGroup::DataValidatorGroup(unsigned siblings) : DataValidatorGroup::~DataValidatorGroup() { while (_first) { - DataValidator* next = _first->sibling(); + DataValidator *next = _first->sibling(); delete (_first); _first = next; } @@ -83,9 +86,11 @@ DataValidatorGroup::~DataValidatorGroup() DataValidator *DataValidatorGroup::add_new_validator() { DataValidator *validator = new DataValidator(); + if (!validator) { return nullptr; } + _last->setSibling(validator); _last = validator; _last->set_timeout(_timeout_interval_us); @@ -101,6 +106,7 @@ DataValidatorGroup::set_timeout(uint32_t timeout_interval_us) next->set_timeout(timeout_interval_us); next = next->sibling(); } + _timeout_interval_us = timeout_interval_us; } @@ -127,12 +133,13 @@ DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64 next->put(timestamp, val, error_count, priority); break; } + next = next->sibling(); i++; } } -float* +float * DataValidatorGroup::get_best(uint64_t timestamp, int *index) { DataValidator *next = _first; @@ -162,9 +169,10 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index) * 2) the confidence is no less than 1% different and the priority is higher */ if ((((max_confidence < MIN_REGULAR_CONFIDENCE) && (confidence >= MIN_REGULAR_CONFIDENCE)) || - (confidence > max_confidence && (next->priority() >= max_priority)) || - (fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority)) - ) && (confidence > 0.0f)) { + (confidence > max_confidence && (next->priority() >= max_priority)) || + (fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority)) + ) && (confidence > 0.0f)) { + max_index = i; max_confidence = confidence; max_priority = next->priority(); @@ -183,7 +191,7 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index) /* check whether the switch was a failsafe or preferring a higher priority sensor */ if (pre_check_prio != -1 && pre_check_prio < max_priority && - fabsf(pre_check_confidence - max_confidence) < 0.1f) { + fabsf(pre_check_confidence - max_confidence) < 0.1f) { /* this is not a failover */ true_failsafe = false; /* reset error flags, this is likely a hotplug sensor coming online late */ @@ -193,6 +201,7 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index) /* if we're no initialized, initialize the bookkeeping but do not count a failsafe */ if (_curr_best < 0) { _prev_best = max_index; + } else { /* we were initialized before, this is a real failsafe */ _prev_best = pre_check_best; @@ -210,6 +219,7 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index) /* for all cases we want to keep a record of the best index */ _curr_best = max_index; } + *index = max_index; return (best) ? best->value() : nullptr; } @@ -225,7 +235,7 @@ DataValidatorGroup::get_vibration_factor(uint64_t timestamp) while (next != nullptr) { if (next->confidence(timestamp) > 0.5f) { - float* rms = next->rms(); + float *rms = next->rms(); for (unsigned j = 0; j < 3; j++) { if (rms[j] > vibe) { @@ -251,7 +261,7 @@ DataValidatorGroup::get_vibration_offset(uint64_t timestamp, int axis) while (next != nullptr) { if (next->confidence(timestamp) > 0.5f) { - float* vibration_offset = next->vibration_offset(); + float *vibration_offset = next->vibration_offset(); if (vibe < 0.0f || vibration_offset[axis] < vibe) { vibe = vibration_offset[axis]; @@ -269,8 +279,8 @@ DataValidatorGroup::print() { /* print the group's state */ ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (%u events)", - _curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO", - _toggle_count); + _curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO", + _toggle_count); DataValidator *next = _first; unsigned i = 0; @@ -280,15 +290,16 @@ DataValidatorGroup::print() uint32_t flags = next->state(); ECL_INFO("sensor #%u, prio: %d, state:%s%s%s%s%s%s", i, next->priority(), - ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), - ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""), - ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TOUT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ECNT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " EDNST" : ""), - ((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : "")); + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TOUT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ECNT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " EDNST" : ""), + ((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : "")); next->print(); } + next = next->sibling(); i++; } @@ -310,9 +321,11 @@ DataValidatorGroup::failover_index() if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) { return i; } + next = next->sibling(); i++; } + return -1; } @@ -326,8 +339,10 @@ DataValidatorGroup::failover_state() if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) { return next->state(); } + next = next->sibling(); i++; } + return DataValidator::ERROR_FLAG_NO_ERROR; } diff --git a/validation/data_validator_group.h b/validation/data_validator_group.h index b6b387ab3a..34d54d4654 100644 --- a/validation/data_validator_group.h +++ b/validation/data_validator_group.h @@ -43,7 +43,8 @@ #include "data_validator.h" -class DataValidatorGroup { +class DataValidatorGroup +{ public: /** * @param siblings initial number of DataValidator's. Must be > 0. @@ -66,15 +67,14 @@ public: * @param error_count The current error count of the sensor * @param priority The priority of the sensor */ - void put(unsigned index, uint64_t timestamp, - float val[3], uint64_t error_count, int priority); + void put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count, int priority); /** * Get the best data triplet of the group * * @return pointer to the array of best values */ - float* get_best(uint64_t timestamp, int *index); + float *get_best(uint64_t timestamp, int *index); /** * Get the RMS / vibration factor @@ -143,6 +143,6 @@ private: static constexpr float MIN_REGULAR_CONFIDENCE = 0.9f; /* we don't want this class to be copied */ - DataValidatorGroup(const DataValidatorGroup&); - DataValidatorGroup operator=(const DataValidatorGroup&); + DataValidatorGroup(const DataValidatorGroup &); + DataValidatorGroup operator=(const DataValidatorGroup &); };