diff --git a/CMakeLists.txt b/CMakeLists.txt index a5fb4b2437..af1f6c36be 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -90,19 +90,29 @@ if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR) elseif(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX) add_compile_options( -pedantic - + -Wall -Wextra -Werror -Wno-missing-field-initializers # ignore for GCC 4.8 support ) + + # enable color output + if(CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 4.9) + # force color for gcc > 4.9 + add_compile_options(-fdiagnostics-color=always) + endif() + + elseif ("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang") + # force color for clang (needed for clang + ccache) + add_compile_options(-fcolor-diagnostics) endif() # testing include(CTest) enable_testing() - + if(BUILD_TESTING) option(ECL_TESTS "Build ECL tests" ON) @@ -196,17 +206,17 @@ endif() if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR) option(BUILD_DOXYGEN "Build doxygen documentation" OFF) - + if (BUILD_DOXYGEN) find_package(Doxygen) if (DOXYGEN_FOUND) # set input and output files set(DOXYGEN_IN ${CMAKE_CURRENT_SOURCE_DIR}/docs/Doxyfile.in) set(DOXYGEN_OUT ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile) - + # request to configure the file configure_file(${DOXYGEN_IN} ${DOXYGEN_OUT} @ONLY) - + # note the option ALL which allows to build the docs together with the application add_custom_target(doxygen ALL COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYGEN_OUT} @@ -216,7 +226,7 @@ if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR) VERBATIM USES_TERMINAL ) - + else() message(FATAL_ERROR "Doxygen needs to be installed to generate documentation") endif() diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index a5b322b42b..59be33636c 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -40,10 +40,15 @@ * @author Paul Riseborough * */ -#include "../ecl.h" + +#include + #include "ekf.h" #include +namespace estimator +{ + void Ekf::fuseAirspeed() { // Initialize variables @@ -274,3 +279,5 @@ void Ekf::resetWindStates() _state.wind_vel(1) = 0.0f; } } + +} // namespace estimator diff --git a/EKF/common.h b/EKF/common.h index c5ecaf81ea..f20ae3a518 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -31,6 +31,8 @@ * ****************************************************************************/ +#pragma once + /** * @file common.h * Definition of base class for attitude estimators diff --git a/EKF/control.cpp b/EKF/control.cpp index 6db7dc1dc2..adac00d378 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -39,10 +39,14 @@ * */ -#include "../ecl.h" +#include + #include "ekf.h" #include +namespace estimator +{ + void Ekf::controlFusionModes() { // Store the status to enable change detection @@ -71,6 +75,7 @@ void Ekf::controlFusionModes() } 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); + } else { ECL_ERR("EKF aligned, (unknown height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length); } @@ -166,7 +171,10 @@ 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_EVVEL)) && !_control_status.flags.ev_yaw) { + if ((_params.fusion_mode & MASK_ROTATE_EV) + && ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVVEL)) + && !_control_status.flags.ev_yaw) + { // rotate EV measurements into the EKF Navigation frame calcExtVisRotMat(); } @@ -190,8 +198,10 @@ void Ekf::controlExternalVisionFusion() ECL_INFO_TIMESTAMPED("EKF commencing external vision velocity fusion"); } - if ((_params.fusion_mode & MASK_ROTATE_EV) && !(_params.fusion_mode & MASK_USE_EVYAW) - && !_ev_rot_mat_initialised) { + if ((_params.fusion_mode & MASK_ROTATE_EV) + && !(_params.fusion_mode & MASK_USE_EVYAW) + && !_ev_rot_mat_initialised) + { // Reset transformation between EV and EKF navigation frames when starting fusion resetExtVisRotMat(); _ev_rot_mat_initialised = true; @@ -201,9 +211,15 @@ void Ekf::controlExternalVisionFusion() } // external vision yaw aiding selection logic - if (!_control_status.flags.gps && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) { + if (!_control_status.flags.gps + && (_params.fusion_mode & MASK_USE_EVYAW) + && !_control_status.flags.ev_yaw + && _control_status.flags.tilt_align) + { + // don't start using EV data unless daa is arriving frequently if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { + // reset the yaw angle to the value from the observation quaternion // get the roll, pitch, yaw estimates from the quaternion states Quatf q_init(_state.quat_nominal); @@ -286,6 +302,7 @@ void Ekf::controlExternalVisionFusion() // Use an incremental position fusion method for EV position data if GPS is also used if (_params.fusion_mode & MASK_USE_GPS) { _fuse_hpos_as_odom = true; + } else { _fuse_hpos_as_odom = false; } @@ -319,9 +336,11 @@ void Ekf::controlExternalVisionFusion() } else { // use the absolute position Vector3f ev_pos_meas = _ev_sample_delayed.pos; + if (_params.fusion_mode & MASK_ROTATE_EV) { ev_pos_meas = _ev_rot_mat * ev_pos_meas; } + _vel_pos_innov[3] = _state.pos(0) - ev_pos_meas(0); _vel_pos_innov[4] = _state.pos(1) - ev_pos_meas(1); // observation 1-STD error @@ -330,7 +349,9 @@ void Ekf::controlExternalVisionFusion() // check if we have been deadreckoning too long if ((_time_last_imu - _time_last_pos_fuse) > _params.reset_timeout_max) { // don't reset velocity if we have another source of aiding constraining it - if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) && ((_time_last_imu - _time_last_vel_fuse) > (uint64_t)1E6)) { + if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) + && ((_time_last_imu - _time_last_vel_fuse) > (uint64_t)1E6)) { + resetVelocity(); } @@ -340,7 +361,8 @@ void Ekf::controlExternalVisionFusion() // innovation gate size _posInnovGateNE = fmaxf(_params.ev_pos_innov_gate, 1.0f); - }else{ + + } else { _vel_pos_innov[3] = 0.0f; _vel_pos_innov[4] = 0.0f; } @@ -371,7 +393,8 @@ void Ekf::controlExternalVisionFusion() // check if we have been deadreckoning too long if ((_time_last_imu - _time_last_vel_fuse) > _params.reset_timeout_max) { // don't reset velocity if we have another source of aiding constraining it - if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) && ((_time_last_imu - _time_last_pos_fuse) > (uint64_t)1E6)) { + if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) + && ((_time_last_imu - _time_last_pos_fuse) > (uint64_t)1E6)) { resetVelocity(); } } @@ -419,9 +442,9 @@ void Ekf::controlOpticalFlowFusion() const float accel_norm = _accel_vec_filt.norm(); const bool motion_is_excessive = ((accel_norm > (CONSTANTS_ONE_G * 1.5f)) // upper g limit - || (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit - || (_ang_rate_mag_filt > _flow_max_rate) // angular rate exceeds flow sensor limit - || (_R_to_earth(2,2) < cosf(math::radians(30.0f)))); // tilted excessively + || (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit + || (_ang_rate_mag_filt > _flow_max_rate) // angular rate exceeds flow sensor limit + || (_R_to_earth(2, 2) < cosf(math::radians(30.0f)))); // tilted excessively if (motion_is_excessive) { _time_bad_motion_us = _imu_sample_delayed.time_us; @@ -444,29 +467,37 @@ void Ekf::controlOpticalFlowFusion() // Inhibit flow use if motion is un-suitable or we have good quality GPS // Apply hysteresis to prevent rapid mode switching float gps_err_norm_lim; + if (_control_status.flags.opt_flow) { gps_err_norm_lim = 0.7f; + } else { gps_err_norm_lim = 1.0f; } // Check if we are in-air and require optical flow to control position drift bool flow_required = _control_status.flags.in_air && - (_is_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently - || (_control_status.flags.opt_flow && !_control_status.flags.gps && !_control_status.flags.ev_pos && !_control_status.flags.ev_vel) // is completely reliant on optical flow - || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad + (_is_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently + || (_control_status.flags.opt_flow && !_control_status.flags.gps && !_control_status.flags.ev_pos + && !_control_status.flags.ev_vel) // is completely reliant on optical flow + || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad if (!_inhibit_flow_use && _control_status.flags.opt_flow) { // inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation - bool preflight_motion_not_ok = !_control_status.flags.in_air && ((_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5); + bool preflight_motion_not_ok = !_control_status.flags.in_air + && ((_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5); bool flight_motion_not_ok = _control_status.flags.in_air && !_range_aid_mode_enabled; + if ((preflight_motion_not_ok || flight_motion_not_ok) && !flow_required) { _inhibit_flow_use = true; } - } else if (_inhibit_flow_use && !_control_status.flags.opt_flow){ + + } else if (_inhibit_flow_use && !_control_status.flags.opt_flow) { // allow use of optical flow if motion is suitable or we are reliant on it for flight navigation - bool preflight_motion_ok = !_control_status.flags.in_air && ((_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6); + bool preflight_motion_ok = !_control_status.flags.in_air + && ((_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6); bool flight_motion_ok = _control_status.flags.in_air && _range_aid_mode_enabled; + if (preflight_motion_ok || flight_motion_ok || flow_required) { _inhibit_flow_use = false; } @@ -475,9 +506,9 @@ 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_flow_use) { - _control_status.flags.opt_flow = false; - _time_last_of_fuse = 0; + if (_inhibit_flow_use) { + _control_status.flags.opt_flow = false; + _time_last_of_fuse = 0; } else if ((_time_last_imu - _time_last_of_fuse) > (uint64_t)_params.reset_timeout_max) { _control_status.flags.opt_flow = false; @@ -487,11 +518,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 - && !_inhibit_flow_use - && 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 + && !_inhibit_flow_use + && 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); @@ -505,6 +535,7 @@ void Ekf::controlOpticalFlowFusion() // if we are not using GPS or external vision aiding, then the velocity and position states and covariances need to be set const bool flow_aid_only = !(_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel); + if (flow_aid_only) { resetVelocity(); resetPosition(); @@ -540,11 +571,13 @@ void Ekf::controlOpticalFlowFusion() if (!flow_quality_good && !_control_status.flags.in_air) { // when on the ground with poor flow quality, assume zero ground relative velocity and LOS rate _flowRadXYcomp.zero(); + } else { // compensate for body motion to give a LOS rate _flowRadXYcomp(0) = _flow_sample_delayed.flowRadXY(0) - _flow_sample_delayed.gyroXYZ(0); _flowRadXYcomp(1) = _flow_sample_delayed.flowRadXY(1) - _flow_sample_delayed.gyroXYZ(1); } + } else { // don't use this flow data and wait for the next data to arrive _flow_data_ready = false; @@ -552,7 +585,8 @@ void Ekf::controlOpticalFlowFusion() } // Wait until the midpoint of the flow sample has fallen behind the fusion time horizon - if (_flow_data_ready && (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) { + if (_flow_data_ready + && (_imu_sample_delayed.time_us > _flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) { // Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently // but use a relaxed time criteria to enable it to coast through bad range finder data if (_control_status.flags.opt_flow && ((_time_last_imu - _time_last_hagl_fuse) < (uint64_t)10e6)) { @@ -572,10 +606,10 @@ void Ekf::controlGpsFusion() // GPS yaw aiding selection logic if ((_params.fusion_mode & MASK_USE_GPSYAW) - && ISFINITE(_gps_sample_delayed.yaw) - && _control_status.flags.tilt_align - && (!_control_status.flags.gps_yaw || !_control_status.flags.yaw_align) - && ((_time_last_imu - _time_last_gps) < (2 * GPS_MAX_INTERVAL))) { + && ISFINITE(_gps_sample_delayed.yaw) + && _control_status.flags.tilt_align + && (!_control_status.flags.gps_yaw || !_control_status.flags.yaw_align) + && ((_time_last_imu - _time_last_gps) < (2 * GPS_MAX_INTERVAL))) { if (resetGpsAntYaw()) { // flag the yaw as aligned @@ -606,6 +640,7 @@ void Ekf::controlGpsFusion() // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently bool gps_checks_passing = (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6); bool gps_checks_failing = (_time_last_imu - _last_gps_pass_us > (uint64_t)5e6); + if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) { if (_control_status.flags.tilt_align && _NED_origin_initialised && gps_checks_passing) { // If the heading is not aligned, reset the yaw and magnetic field states @@ -614,6 +649,7 @@ void Ekf::controlGpsFusion() if (!_control_status.flags.yaw_align || _control_status.flags.ev_yaw || _mag_inhibit_yaw_reset_req) { _control_status.flags.ev_yaw = false; _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + // Handle the special case where we have not been constraining yaw drift or learning yaw bias due // to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor. if (_mag_inhibit_yaw_reset_req) { @@ -653,12 +689,15 @@ void Ekf::controlGpsFusion() } // Handle the case where we are using GPS and another source of aiding and GPS is failing checks - if (_control_status.flags.gps && gps_checks_failing && (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel)) { + if (_control_status.flags.gps && gps_checks_failing && (_control_status.flags.opt_flow || _control_status.flags.ev_pos + || _control_status.flags.ev_vel)) { _control_status.flags.gps = false; + // Reset position state to external vision if we are going to use absolute values if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) { resetPosition(); } + ECL_WARN_TIMESTAMPED("EKF GPS data quality poor - stopping use"); } @@ -744,7 +783,11 @@ void Ekf::controlGpsFusion() } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) { _control_status.flags.gps = false; ECL_WARN_TIMESTAMPED("EKF GPS data stopped"); - } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel)) { + + } else if (_control_status.flags.gps + && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) + && (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel)) { + // Handle the case where we are fusing another position source along GPS, // stop waiting for GPS after 1 s of lost signal _control_status.flags.gps = false; @@ -1178,6 +1221,7 @@ void Ekf::rangeAidConditionsMet() if (_control_status.flags.in_air && !_rng_hgt_faulty) { // check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching bool can_use_range_finder; + if (_range_aid_mode_enabled) { can_use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && get_terrain_valid(); @@ -1239,6 +1283,7 @@ void Ekf::checkRangeDataValidity() // Don't run the checks after this unless we have retrieved new data from the buffer if (!_range_data_ready) { return; + } else { // reset fault status when we get new data _rng_hgt_faulty = (_range_sample_delayed.quality == 0); @@ -1252,10 +1297,12 @@ void Ekf::checkRangeDataValidity() // Check if out of range if ((_range_sample_delayed.rng > _rng_valid_max_val) - || (_range_sample_delayed.rng < _rng_valid_min_val)) { + || (_range_sample_delayed.rng < _rng_valid_min_val)) { + if (_control_status.flags.in_air) { _rng_hgt_faulty = true; return; + } else { // Range finders can fail to provide valid readings when resting on the ground // or being handled by the user, which prevents use of as a primary height sensor. @@ -1375,8 +1422,8 @@ void Ekf::controlBetaFusion() void Ekf::controlDragFusion() { if (_params.fusion_mode & MASK_USE_DRAG) { - if (_control_status.flags.in_air - && !_mag_inhibit_yaw_reset_req) { + if (_control_status.flags.in_air && !_mag_inhibit_yaw_reset_req) { + if (!_control_status.flags.wind) { // reset the wind states and covariances when starting drag accel fusion _control_status.flags.wind = true; @@ -1405,6 +1452,7 @@ void Ekf::controlMagFusion() save_mag_cov_data(); _control_status.flags.mag_3D = false; } + zeroRows(P, 16, 21); zeroCols(P, 16, 21); _mag_decl_cov_reset = false; @@ -1436,7 +1484,7 @@ void Ekf::controlMagFusion() // perform a yaw reset if requested by other functions if (_mag_yaw_reset_req && _control_status.flags.tilt_align) { - if (!_mag_use_inhibit ) { + if (!_mag_use_inhibit) { if (!_control_status.flags.mag_align_complete && _control_status.flags.fixed_wing && _control_status.flags.in_air) { // A fixed wing vehicle can use GPS to bound yaw errors immediately after launch _control_status.flags.mag_align_complete = realignYawGPS(); @@ -1451,6 +1499,7 @@ void Ekf::controlMagFusion() _control_status.flags.mag_align_complete = resetMagHeading(_mag_sample_delayed.mag) && _control_status.flags.in_air; } } + _control_status.flags.yaw_align = _control_status.flags.yaw_align || _control_status.flags.mag_align_complete; _mag_yaw_reset_req = false; } @@ -1472,7 +1521,8 @@ void Ekf::controlMagFusion() _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); // Do we have to add ev_vel here? + // Do we have to add ev_vel here? + _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 if (!_mag_bias_observable && (fabsf(_yaw_rate_lpf_ef) > _params.mag_yaw_rate_gate)) { @@ -1519,6 +1569,7 @@ void Ekf::controlMagFusion() for (uint8_t index = 0; index <= 3; index ++) { P[index + 18][index + 18] = _saved_mag_bf_variance[index]; } + // re-instate the NE axis covariance sub-matrix for (uint8_t row = 0; row <= 1; row ++) { for (uint8_t col = 0; col <= 1; col ++) { @@ -1555,6 +1606,7 @@ void Ekf::controlMagFusion() _flt_mag_align_converging = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) < (uint64_t)5e6); if (_control_status.flags.mag_3D && _control_status_prev.flags.update_mag_states_only && !_control_status.flags.update_mag_states_only) { + // When re-commencing use of magnetometer to correct vehicle states // set the field state variance to the observation variance and zero // the covariance terms to allow the field states re-learn rapidly @@ -1585,6 +1637,7 @@ void Ekf::controlMagFusion() // only commence 3-axis fusion when yaw is aligned and field states set _control_status.flags.mag_3D = true; } + } else { // do no magnetometer fusion at all _control_status.flags.mag_hdg = false; @@ -1614,8 +1667,10 @@ void Ekf::controlMagFusion() bool not_using_evpos = !(_params.fusion_mode & MASK_USE_EVPOS) || !_control_status.flags.ev_pos; bool not_using_evvel = !(_params.fusion_mode & MASK_USE_EVVEL) || !_control_status.flags.ev_vel; bool not_selected_evyaw = !(_params.fusion_mode & MASK_USE_EVYAW); + if (user_selected && not_using_gps && not_using_evpos && not_using_evvel && not_selected_evyaw) { _mag_use_inhibit = true; + } else { _mag_use_inhibit = false; _mag_use_not_inhibit_us = _imu_sample_delayed.time_us; @@ -1636,11 +1691,13 @@ void Ekf::controlMagFusion() fuseDeclination(0.02f); _mag_decl_cov_reset = true; fuseMag(); + } else { // The normal sequence is to fuse the magnetometer data first before fusing // declination angle at a higher uncertainty to allow some learning of // declination angle over time. fuseMag(); + if (_control_status.flags.mag_dec) { fuseDeclination(0.5f); } @@ -1737,3 +1794,5 @@ void Ekf::controlAuxVelFusion() fuseVelPosHeight(); } } + +} // namespace estimator diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 76e56e5d10..dae1514c66 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -46,6 +46,9 @@ #include #include +namespace estimator +{ + void Ekf::initialiseCovariance() { for (unsigned i = 0; i < _k_num_states; i++) { @@ -439,8 +442,8 @@ void Ekf::predictCovariance() // process noise contribution for delta angle states can be very small compared to // the variances, therefore use algorithm to minimise numerical error - for (unsigned i = 10; i <=12; i++) { - const int index = i-10; + for (unsigned i = 10; i <= 12; i++) { + const int index = i - 10; nextP[i][i] = kahanSummation(nextP[i][i], process_noise[i], _delta_angle_bias_var_accum(index)); } @@ -498,7 +501,7 @@ void Ekf::predictCovariance() // process noise contributiton for delta velocity states can be very small compared to // the variances, therefore use algorithm to minimise numerical error for (unsigned i = 13; i <= 15; i++) { - const int index = i-13; + const int index = i - 13; nextP[i][i] = kahanSummation(nextP[i][i], process_noise[i], _delta_vel_bias_var_accum(index)); } @@ -957,3 +960,6 @@ void Ekf::resetWindCovariance() } } + +} // namespace estimator + diff --git a/EKF/drag_fusion.cpp b/EKF/drag_fusion.cpp index b2d14fe43f..567c61ce44 100644 --- a/EKF/drag_fusion.cpp +++ b/EKF/drag_fusion.cpp @@ -43,6 +43,9 @@ #include #include +namespace estimator +{ + void Ekf::fuseDrag() { float SH_ACC[4] = {}; // Variable used to optimise calculations of measurement jacobian @@ -312,3 +315,6 @@ void Ekf::fuseDrag() } } } + +} // namespace estimator + diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 7e6ffe23ac..ade342dde1 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -44,6 +44,9 @@ #include #include +namespace estimator +{ + bool Ekf::init(uint64_t timestamp) { bool ret = initialise_interface(timestamp); @@ -263,6 +266,7 @@ bool Ekf::initialiseFilter() if (_control_status.flags.ev_yaw) { // using error estimate from external vision data TODO: this is never true increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f))); + } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) { // using magnetic heading tuning parameter increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f))); @@ -673,3 +677,5 @@ Quatf Ekf::calculate_quaternion() const // the quaternions must always be normalised after modification return Quatf{_output_new.quat_nominal * AxisAnglef{delta_angle}}.unit(); } + +} // namespace estimator diff --git a/EKF/ekf.h b/EKF/ekf.h index 26b8487ec1..e29b2d328d 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -44,6 +44,9 @@ #include "estimator_interface.h" +namespace estimator +{ + class Ekf : public EstimatorInterface { public: @@ -560,7 +563,7 @@ class Ekf : public EstimatorInterface // reset the heading and magnetic field states using the declination and magnetometer/external vision measurements // return true if successful - bool resetMagHeading(Vector3f &mag_init, bool increase_yaw_var = true, bool update_buffer=true); + bool resetMagHeading(Vector3f &mag_init, bool increase_yaw_var = true, bool update_buffer = true); // Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS. // It is used to align the yaw angle after launch or takeoff for fixed wing vehicle. @@ -655,7 +658,8 @@ class Ekf : public EstimatorInterface void checkRangeDataValidity(); // return the square of two floating point numbers - used in auto coded sections - static constexpr float sq(float var) { return var * var; } + template + static constexpr T sq(T var) { return var * var; } // set control flags to use baro height void setControlBaroHeight(); @@ -720,3 +724,5 @@ class Ekf : public EstimatorInterface float kahanSummation(float sum_previous, float input, float &accumulator) const; }; + +} // namespace estimator diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index eafd8b767a..8fc429098c 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -39,12 +39,16 @@ * */ +#include + #include "ekf.h" -#include #include #include +namespace estimator +{ + // Reset the velocity states. If we have a recent and valid // gps measurement then use for velocity initialisation bool Ekf::resetVelocity() @@ -180,8 +184,8 @@ bool Ekf::resetPosition() } // estimate is relative to initial position in this mode, so we start with zero error. - zeroCols(P,7,8); - zeroRows(P,7,8); + zeroCols(P, 7, 8); + zeroRows(P, 7, 8); } else { // Used when falling back to non-aiding mode of operation @@ -581,6 +585,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update save_mag_cov_data(); _control_status.flags.mag_3D = false; } + zeroRows(P, 16, 21); zeroCols(P, 16, 21); _mag_decl_cov_reset = false; @@ -751,6 +756,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update if (_control_status.flags.ev_yaw) { // using error estimate from external vision data increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f))); + } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) { // using magnetic heading tuning parameter increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f))); @@ -1022,10 +1028,12 @@ bool Ekf::get_gps_drift_metrics(float drift[3], bool *blocked) { memcpy(drift, _gps_drift_metrics, 3 * sizeof(float)); *blocked = !_vehicle_at_rest; + if (_gps_drift_updated) { _gps_drift_updated = false; return true; } + return false; } @@ -1286,10 +1294,9 @@ void Ekf::zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first void Ekf::zeroOffDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last) { // save diagonal elements - uint8_t row; float variances[_k_num_states]; - for (row = first; row <= last; row++) { + for (uint8_t row = first; row <= last; row++) { variances[row] = cov_mat[row][row]; } @@ -1298,7 +1305,7 @@ void Ekf::zeroOffDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t fi zeroCols(cov_mat, first, last); // restore diagonals - for (row = first; row <= last; row++) { + for (uint8_t row = first; row <= last; row++) { cov_mat[row][row] = variances[row]; } } @@ -1309,6 +1316,7 @@ void Ekf::uncorrelateQuatStates() uint32_t row; uint32_t col; float variances[4][4]; + for (row = 0; row < 4; row++) { for (col = 0; col < 4; col++) { variances[row][col] = P[row][col]; @@ -1461,7 +1469,8 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var) { // calculate an equivalent rotation vector from the quaternion float q0,q1,q2,q3; - if (_state.quat_nominal(0) >= 0.0f) { + + if (_state.quat_nominal(0) >= 0.0) { q0 = _state.quat_nominal(0); q1 = _state.quat_nominal(1); q2 = _state.quat_nominal(2); @@ -1742,3 +1751,6 @@ float Ekf::kahanSummation(float sum_previous, float input, float &accumulator) c accumulator = (t - sum_previous) - y; return t; } + +} // namespace estimator + diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index e10130d7db..2f4ad3b824 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -45,6 +45,9 @@ #include #include +namespace estimator +{ + // Accumulate imu data and store to buffer at desired rate void EstimatorInterface::setIMUData(const imuSample &imu_sample) { @@ -78,8 +81,8 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample) // detect if the vehicle is not moving when on ground if (!_control_status.flags.in_air) { if ((_vibe_metrics[1] * 4.0E4f > _params.is_moving_scaler) - || (_vibe_metrics[2] * 2.1E2f > _params.is_moving_scaler) - || ((imu_sample.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) { + || (_vibe_metrics[2] * 2.1E2f > _params.is_moving_scaler) + || ((imu_sample.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) { _time_last_move_detect_us = imu_sample.time_us; } @@ -228,8 +231,10 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, const gps_message &gps) gps_sample_new.hgt = (float)gps.alt * 1e-3f; gps_sample_new.yaw = gps.yaw; + if (ISFINITE(gps.yaw_offset)) { _gps_yaw_offset = gps.yaw_offset; + } else { _gps_yaw_offset = 0.0f; } @@ -379,6 +384,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl // use this to prevent use of a saturated flow sensor when there are other aiding sources available float flow_rate_magnitude; bool flow_magnitude_good = true; + if (delta_time_good) { flow_rate_magnitude = flow->flowdata.norm() / delta_time; flow_magnitude_good = (flow_rate_magnitude <= _flow_max_rate); @@ -392,6 +398,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl // Check data validity and write to buffers // Invalid flow data is allowed when on ground and is handled as a special case in controlOpticalFlowFusion() bool use_flow_data_to_navigate = delta_time_good && flow_quality_good && (flow_magnitude_good || relying_on_flow); + if (use_flow_data_to_navigate || (!_control_status.flags.in_air && relying_on_flow)) { flowSample optflow_sample_new; // calculate the system time-stamp for the trailing edge of the flow data integration period @@ -584,3 +591,6 @@ void EstimatorInterface::print_status() ECL_INFO("output vert buffer: %d (%d Bytes)", _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size()); ECL_INFO("drag buffer: %d (%d Bytes)", _drag_buffer.get_length(), _drag_buffer.get_total_size()); } + +} // namespace estimator + diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 6139a2ebfa..ee9e77ca3c 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -49,7 +49,8 @@ #include #include -using namespace estimator; +namespace estimator +{ class EstimatorInterface { @@ -329,6 +330,7 @@ class EstimatorInterface bool get_mag_decl_deg(float *val) { *val = 0.0f; + if (_NED_origin_initialised && (_params.mag_declination_source & MASK_SAVE_GEO_DECL)) { *val = math::degrees(_mag_declination_gps); return true; @@ -469,8 +471,8 @@ class EstimatorInterface bool _gps_speed_valid{false}; float _gps_origin_eph{0.0f}; // horizontal position uncertainty of the GPS origin float _gps_origin_epv{0.0f}; // vertical position uncertainty of the GPS origin - struct map_projection_reference_s _pos_ref {}; // Contains WGS-84 position latitude and longitude (radians) of the EKF origin - struct map_projection_reference_s _gps_pos_prev {}; // Contains WGS-84 position latitude and longitude (radians) of the previous GPS message + map_projection_reference_s _pos_ref{}; // Contains WGS-84 position latitude and longitude (radians) of the EKF origin + map_projection_reference_s _gps_pos_prev{}; // Contains WGS-84 position latitude and longitude (radians) of the previous GPS message float _gps_alt_prev{0.0f}; // height from the previous GPS message (m) float _gps_yaw_offset{0.0f}; // Yaw offset angle for dual GPS antennas used for yaw estimation (radians). @@ -564,3 +566,6 @@ class EstimatorInterface Matrix3f quat_to_invrotmat(const Quatf &quat); }; + +} // namespace estimator + diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 43f39ee0e2..bb8b4bf448 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -45,6 +45,9 @@ #include #include +namespace estimator +{ + // GPS pre-flight check bit locations #define MASK_GPS_NSATS (1<<0) #define MASK_GPS_GDOP (1<<1) @@ -60,6 +63,7 @@ bool Ekf::collect_gps(const gps_message &gps) { // Run GPS checks always _gps_checks_passed = gps_is_good(gps); + if (!_NED_origin_initialised && _gps_checks_passed) { // If we have good GPS data set the origin's WGS-84 position to the last gps fix double lat = gps.lat / 1.0e7; @@ -97,6 +101,7 @@ bool Ekf::collect_gps(const gps_message &gps) _control_status.flags.rng_hgt = false; // zero the sensor offset _hgt_sensor_offset = 0.0f; + } else { ECL_INFO_TIMESTAMPED("EKF GPS checks passed (WGS-84 origin set)"); } @@ -132,8 +137,8 @@ bool Ekf::gps_is_good(const gps_message &gps) _gps_check_fail_status.flags.sacc = (gps.sacc > _params.req_sacc); // check if GPS quality is degraded - _gps_error_norm = fmaxf((gps.eph / _params.req_hacc) , (gps.epv / _params.req_vacc)); - _gps_error_norm = fmaxf(_gps_error_norm , (gps.sacc / _params.req_sacc)); + _gps_error_norm = fmaxf((gps.eph / _params.req_hacc), (gps.epv / _params.req_vacc)); + _gps_error_norm = fmaxf(_gps_error_norm, (gps.sacc / _params.req_sacc)); // Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient const float filt_time_const = 10.0f; @@ -143,6 +148,7 @@ bool Ekf::gps_is_good(const gps_message &gps) // The following checks are only valid when the vehicle is at rest double lat = gps.lat * 1.0e-7; double lon = gps.lon * 1.0e-7; + if (!_control_status.flags.in_air && _vehicle_at_rest) { // Calculate position movement since last measurement float delta_posN = 0.0f; @@ -238,6 +244,7 @@ bool Ekf::gps_is_good(const gps_message &gps) (_gps_check_fail_status.flags.vspeed && (_params.gps_check_mask & MASK_GPS_VSPD)) ) { _last_gps_fail_us = _time_last_imu; + } else { _last_gps_pass_us = _time_last_imu; } @@ -245,3 +252,6 @@ bool Ekf::gps_is_good(const gps_message &gps) // continuous period without fail of x seconds required to return a healthy status return _time_last_imu - _last_gps_fail_us > (uint64_t)_min_gps_health_time_us; } + +} // namespace estimator + diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index 13d13415c4..59095540d4 100644 --- a/EKF/gps_yaw_fusion.cpp +++ b/EKF/gps_yaw_fusion.cpp @@ -45,6 +45,9 @@ #include #include +namespace estimator +{ + void Ekf::fuseGpsAntYaw() { // assign intermediate state variables @@ -73,7 +76,7 @@ void Ekf::fuseGpsAntYaw() } // calculate predicted antenna yaw angle - predicted_hdg = atan2f(ant_vec_ef(1),ant_vec_ef(0)); + predicted_hdg = atan2f(ant_vec_ef(1), ant_vec_ef(0)); // calculate observation jacobian float t2 = sinf(_gps_yaw_offset); @@ -304,7 +307,7 @@ bool Ekf::resetGpsAntYaw() return false; } - float predicted_yaw = atan2f(ant_vec_ef(1),ant_vec_ef(0)); + float predicted_yaw = atan2f(ant_vec_ef(1), ant_vec_ef(0)); // get measurement and correct for antenna array yaw offset float measured_yaw = _gps_sample_delayed.yaw + _gps_yaw_offset; @@ -424,3 +427,6 @@ bool Ekf::resetGpsAntYaw() return false; } + +} // namespace estimator + diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 355384b398..aa778d0242 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -44,6 +44,9 @@ #include #include +namespace estimator +{ + void Ekf::fuseMag() { // assign intermediate variables @@ -460,6 +463,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 { @@ -579,6 +583,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); } @@ -619,6 +624,7 @@ void Ekf::fuseHeading() // calculate the innovation and define the innovation gate float innov_gate = math::max(_params.heading_innov_gate, 1.0f); + if (_mag_use_inhibit) { // The magnetometer cannot be trusted but we need to fuse a heading to prevent a badly // conditioned covariance matrix developing over time. @@ -627,6 +633,7 @@ void Ekf::fuseHeading() // predicted heading to use as an observation when movement ceases. _heading_innov = 0.0f; _vehicle_at_rest_prev = false; + } else { // Vehicle is at rest so use the last moving prediction as an observation // to prevent the heading from drifting and to enable yaw gyro bias learning @@ -635,14 +642,17 @@ void Ekf::fuseHeading() _last_static_yaw = predicted_hdg; _vehicle_at_rest_prev = true; } + _heading_innov = predicted_hdg - _last_static_yaw; R_YAW = 0.01f; innov_gate = 5.0f; } + } else { _heading_innov = predicted_hdg - measured_hdg; _last_static_yaw = predicted_hdg; } + _mag_use_inhibit_prev = _mag_use_inhibit; // wrap the innovation to the interval between +-pi @@ -810,7 +820,7 @@ void Ekf::fuseDeclination(float decl_sigma) float t3 = magN*magN; float t4 = t2+t3; // if the horizontal magnetic field is too small, this calculation will be badly conditioned - if (t4 < h_field_min*h_field_min) { + if (t4 < h_field_min * h_field_min) { return; } float t5 = P[16][16]*t2; @@ -824,8 +834,10 @@ void Ekf::fuseDeclination(float decl_sigma) float t15 = P[17][16]*magE*magN; float t12 = t5+t6+t8+t10+t11-t14-t15; float t13; + if (fabsf(t12) > 1e-6f) { t13 = 1.0f / t12; + } else { return; } @@ -833,6 +845,7 @@ void Ekf::fuseDeclination(float decl_sigma) float t19 = magN*magN; float t20 = t18+t19; float t21; + if (fabsf(t20) > 1e-6f) { t21 = 1.0f/t20; } else { @@ -882,6 +895,7 @@ void Ekf::fuseDeclination(float decl_sigma) // take advantage of the empty columns in KH to reduce the number of operations float KHP[_k_num_states][_k_num_states]; float KH[2]; + for (unsigned row = 0; row < _k_num_states; row++) { KH[0] = Kfusion[row] * H_DECL[16]; @@ -898,6 +912,7 @@ void Ekf::fuseDeclination(float decl_sigma) // the covariance matrix is unhealthy and must be corrected bool healthy = true; _fault_status.flags.bad_mag_decl = false; + for (int i = 0; i < _k_num_states; i++) { if (P[i][i] < KHP[i][i]) { // zero rows and columns @@ -939,14 +954,17 @@ void Ekf::limitDeclination() // set to 50% of the horizontal strength from geo tables if location is known float decl_reference; float h_field_min = 0.001f; + 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) { decl_reference = _mag_declination_gps; - h_field_min = fmaxf(h_field_min , 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps)); + h_field_min = fmaxf(h_field_min, 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps)); + } else { decl_reference = math::radians(_params.mag_declination_deg); } + } else { // always use the parameter value decl_reference = math::radians(_params.mag_declination_deg); @@ -955,18 +973,21 @@ void Ekf::limitDeclination() // do not allow the horizontal field length to collapse - this will make the declination fusion badly conditioned // and can result in a reversal of the NE field states which the filter cannot recover from // apply a circular limit - float h_field = sqrtf(_state.mag_I(0)*_state.mag_I(0) + _state.mag_I(1)*_state.mag_I(1)); + float h_field = sqrtf(_state.mag_I(0) * _state.mag_I(0) + _state.mag_I(1) * _state.mag_I(1)); + if (h_field < h_field_min) { if (h_field > 0.001f * h_field_min) { float h_scaler = h_field_min / h_field; _state.mag_I(0) *= h_scaler; _state.mag_I(1) *= h_scaler; + } else { // too small to scale radially so set to expected value float mag_declination = getMagDeclination(); _state.mag_I(0) = 2.0f * h_field_min * cosf(mag_declination); _state.mag_I(1) = 2.0f * h_field_min * sinf(mag_declination); } + h_field = h_field_min; } @@ -974,12 +995,17 @@ void Ekf::limitDeclination() const float decl_tolerance = 0.5f; const float decl_max = decl_reference + decl_tolerance; const float decl_min = decl_reference - decl_tolerance; - const float decl_estimate = atan2f(_state.mag_I(1) , _state.mag_I(0)); + const float decl_estimate = atan2f(_state.mag_I(1), _state.mag_I(0)); + if (decl_estimate > decl_max) { _state.mag_I(0) = h_field * cosf(decl_max); _state.mag_I(1) = h_field * sinf(decl_max); + } else if (decl_estimate < decl_min) { _state.mag_I(0) = h_field * cosf(decl_min); _state.mag_I(1) = h_field * sinf(decl_min); } } + +} // namespace estimator + diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index b892d52199..65c5732ef8 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -45,6 +45,9 @@ #include #include +namespace estimator +{ + void Ekf::fuseOptFlow() { float gndclearance = fmaxf(_params.rng_gnd_clearance, 0.1f); @@ -549,7 +552,7 @@ bool Ekf::calcOptFlowBodyRateComp() return false; } - bool use_flow_sensor_gyro = ISFINITE(_flow_sample_delayed.gyroXYZ(0)) && ISFINITE(_flow_sample_delayed.gyroXYZ(1)) && ISFINITE(_flow_sample_delayed.gyroXYZ(2)); + bool use_flow_sensor_gyro = ISFINITE(_flow_sample_delayed.gyroXYZ(0)) && ISFINITE(_flow_sample_delayed.gyroXYZ(1)) && ISFINITE(_flow_sample_delayed.gyroXYZ(2)); if (use_flow_sensor_gyro) { @@ -565,12 +568,9 @@ bool Ekf::calcOptFlowBodyRateComp() of_body_rate = _flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt); // calculate the bias estimate using a combined LPF and spike filter - _flow_gyro_bias(0) = 0.99f * _flow_gyro_bias(0) + 0.01f * math::constrain((of_body_rate(0) - reference_body_rate(0)), - -0.1f, 0.1f); - _flow_gyro_bias(1) = 0.99f * _flow_gyro_bias(1) + 0.01f * math::constrain((of_body_rate(1) - reference_body_rate(1)), - -0.1f, 0.1f); - _flow_gyro_bias(2) = 0.99f * _flow_gyro_bias(2) + 0.01f * math::constrain((of_body_rate(2) - reference_body_rate(2)), - -0.1f, 0.1f); + _flow_gyro_bias(0) = 0.99f * _flow_gyro_bias(0) + 0.01f * math::constrain((of_body_rate(0) - reference_body_rate(0)), -0.1f, 0.1f); + _flow_gyro_bias(1) = 0.99f * _flow_gyro_bias(1) + 0.01f * math::constrain((of_body_rate(1) - reference_body_rate(1)), -0.1f, 0.1f); + _flow_gyro_bias(2) = 0.99f * _flow_gyro_bias(2) + 0.01f * math::constrain((of_body_rate(2) - reference_body_rate(2)), -0.1f, 0.1f); } } else { @@ -597,8 +597,7 @@ float Ekf::calcOptFlowMeasVar() float weighting = (255.0f - (float)_params.flow_qual_min); if (weighting >= 1.0f) { - weighting = math::constrain(((float)_flow_sample_delayed.quality - (float)_params.flow_qual_min) / weighting, 0.0f, - 1.0f); + weighting = math::constrain(((float)_flow_sample_delayed.quality - (float)_params.flow_qual_min) / weighting, 0.0f, 1.0f); } else { weighting = 0.0f; @@ -609,3 +608,6 @@ float Ekf::calcOptFlowMeasVar() return R_LOS; } + +} // namespace estimator + diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index cdeb269c5a..c09c2a8fef 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -44,6 +44,9 @@ #include #include +namespace estimator +{ + void Ekf::fuseSideslip() { float SH_BETA[13] = {}; // Variable used to optimise calculations of measurement jacobian @@ -283,3 +286,6 @@ void Ekf::fuseSideslip() } } } + +} // namespace estimator + diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 9b64a5902d..90eae2c430 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -43,6 +43,9 @@ #include #include +namespace estimator +{ + bool Ekf::initHagl() { // get most recent range measurement from buffer @@ -55,11 +58,13 @@ bool Ekf::initHagl() _terrain_var = sq(_params.range_noise); // success return true; + } else if (_flow_for_terrain_data_ready) { // initialise terrain vertical position to origin as this is the best guess we have _terrain_vpos = fmaxf(0.0f, _state.pos(2)); _terrain_var = 100.0f; return true; + } else if (!_control_status.flags.in_air) { // if on ground we assume a ground clearance _terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance; @@ -165,15 +170,18 @@ void Ekf::fuseHagl() // record last successful fusion event _time_last_hagl_fuse = _time_last_imu; _innov_check_fail_status.flags.reject_hagl = false; + } else { // If we have been rejecting range data for too long, reset to measurement 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; } } + } else { _innov_check_fail_status.flags.reject_hagl = true; return; @@ -222,7 +230,7 @@ void Ekf::fuseFlowForTerrain() float pred_hagl = _terrain_vpos - _state.pos(2); // Calculate observation matrix for flow around the vehicle x axis - float Hx = vel_body(1) * t0 /(pred_hagl * pred_hagl); + float Hx = vel_body(1) * t0 / (pred_hagl * pred_hagl); // Constrain terrain variance to be non-negative _terrain_var = fmaxf(_terrain_var, 0.0f); @@ -234,7 +242,7 @@ void Ekf::fuseFlowForTerrain() float Kx = _terrain_var * Hx / _flow_innov_var[0]; // calculate prediced optical flow about x axis - float pred_flow_x = vel_body(1) * earth_to_body(2,2) / pred_hagl; + float pred_flow_x = vel_body(1) * earth_to_body(2, 2) / pred_hagl; // calculate flow innovation (x axis) _flow_innov[0] = pred_flow_x - opt_flow_rate(0); @@ -255,7 +263,7 @@ void Ekf::fuseFlowForTerrain() } // Calculate observation matrix for flow around the vehicle y axis - float Hy = -vel_body(0) * t0 /(pred_hagl * pred_hagl); + float Hy = -vel_body(0) * t0 / (pred_hagl * pred_hagl); // Calculuate innovation variance _flow_innov_var[1] = Hy * Hy * _terrain_var + R_LOS; @@ -264,7 +272,7 @@ void Ekf::fuseFlowForTerrain() float Ky = _terrain_var * Hy / _flow_innov_var[1]; // calculate prediced optical flow about y axis - float pred_flow_y = -vel_body(0) * earth_to_body(2,2) / pred_hagl; + float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) / pred_hagl; // calculate flow innovation (y axis) _flow_innov[1] = pred_flow_y - opt_flow_rate(1); @@ -320,7 +328,6 @@ void Ekf::get_hagl_innov(float *hagl_innov) memcpy(hagl_innov, &_hagl_innov, sizeof(_hagl_innov)); } - void Ekf::get_hagl_innov_var(float *hagl_innov_var) { memcpy(hagl_innov_var, &_hagl_innov_var, sizeof(_hagl_innov_var)); @@ -346,3 +353,6 @@ void Ekf::checkRangeDataContinuity() _range_data_continuous = false; } } + +} // namespace estimator + diff --git a/EKF/tests/base/base.cpp b/EKF/tests/base/base.cpp index 45de99d063..840b97a627 100644 --- a/EKF/tests/base/base.cpp +++ b/EKF/tests/base/base.cpp @@ -42,6 +42,8 @@ #include #include +using namespace estimator; + int main(int argc, char *argv[]) { (void)argc; // unused diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index ac63e96395..7517997225 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -45,6 +45,9 @@ #include #include +namespace estimator +{ + void Ekf::fuseVelPosHeight() { bool fuse_map[6] = {}; // map of booleans true when [VN,VE,VD,PN,PE,PD] observations are available @@ -140,7 +143,9 @@ void Ekf::fuseVelPosHeight() innovation[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2, _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); + 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); @@ -179,8 +184,12 @@ void Ekf::fuseVelPosHeight() // always pass height checks if yet to complete tilt alignment bool vel_check_pass = (_vel_pos_test_ratio[0] <= 1.0f) && (_vel_pos_test_ratio[1] <= 1.0f) && (_vel_pos_test_ratio[2] <= 1.0f); + innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = vel_check_pass; - bool pos_check_pass = ((_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f)) || !_control_status.flags.tilt_align; + + bool pos_check_pass = ((_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f)) + || !_control_status.flags.tilt_align; + innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass; innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f) || !_control_status.flags.tilt_align; @@ -318,3 +327,6 @@ void Ekf::fuseVelPosHeight() } } } + +} // namespace estimator +