From 9e6d27fafbe52bea3969cdb7bc06b43c0d56e44b Mon Sep 17 00:00:00 2001 From: kamilritz Date: Wed, 25 Dec 2019 17:13:05 +0100 Subject: [PATCH] Add missing const qualifier --- EKF/airspeed_fusion.cpp | 35 ++++++++----------- EKF/control.cpp | 68 +++++++++++++++++-------------------- EKF/covariance.cpp | 47 +++++++++++++------------ EKF/drag_fusion.cpp | 42 +++++++++++------------ EKF/ekf.cpp | 6 ++-- EKF/ekf_helper.cpp | 53 ++++++++++++++--------------- EKF/estimator_interface.cpp | 7 ++-- EKF/gps_checks.cpp | 12 +++---- EKF/gps_yaw_fusion.cpp | 18 +++++----- EKF/mag_fusion.cpp | 53 ++++++++++++++--------------- EKF/optflow_fusion.cpp | 47 ++++++++++++------------- EKF/sideslip_fusion.cpp | 20 +++++------ EKF/terrain_estimator.cpp | 54 ++++++++++++++--------------- EKF/vel_pos_fusion.cpp | 8 ++--- 14 files changed, 225 insertions(+), 245 deletions(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 867b3934d8..94319d531a 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -46,34 +46,28 @@ void Ekf::fuseAirspeed() { - // Initialize variables - float vn; // Velocity in north direction - float ve; // Velocity in east direction - float vd; // Velocity in downwards direction - float vwn; // Wind speed in north direction - float vwe; // Wind speed in east direction - float v_tas_pred; // Predicted measurement - float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, - 10.0f)); // Variance for true airspeed measurement - (m/sec)^2 float SH_TAS[3] = {}; // Variable used to optimise calculations of measurement jacobian float H_TAS[24] = {}; // Observation Jacobian float SK_TAS[2] = {}; // Variable used to optimise calculations of the Kalman gain vector float Kfusion[24] = {}; // Kalman gain vector - // Copy required states to local variable names - vn = _state.vel(0); - ve = _state.vel(1); - vd = _state.vel(2); - vwn = _state.wind_vel(0); - vwe = _state.wind_vel(1); + const float vn = _state.vel(0); // Velocity in north direction + const float ve = _state.vel(1); // Velocity in east direction + const float vd = _state.vel(2); // Velocity in downwards direction + const float vwn = _state.wind_vel(0); // Wind speed in north direction + const float vwe = _state.wind_vel(1); // Wind speed in east direction // Calculate the predicted airspeed - v_tas_pred = sqrtf((ve - vwe) * (ve - vwe) + (vn - vwn) * (vn - vwn) + vd * vd); + const float v_tas_pred = sqrtf((ve - vwe) * (ve - vwe) + (vn - vwn) * (vn - vwn) + vd * vd); + + // Variance for true airspeed measurement - (m/sec)^2 + const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * + math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f)); // Perform fusion of True Airspeed measurement if (v_tas_pred > 1.0f) { // determine if we need the sideslip fusion to correct states other than wind - bool update_wind_only = !_is_wind_dead_reckoning; + const bool update_wind_only = !_is_wind_dead_reckoning; // Calculate the observation jacobian // intermediate variable from algebraic optimisation @@ -90,7 +84,7 @@ void Ekf::fuseAirspeed() H_TAS[23] = -SH_TAS[1]; // We don't want to update the innovation variance if the calculation is ill conditioned - float _airspeed_innov_var_temp = (R_TAS + SH_TAS[2]*(P(4,4)*SH_TAS[2] + P(5,4)*SH_TAS[1] - P(22,4)*SH_TAS[2] - P(23,4)*SH_TAS[1] + P(6,4)*vd*SH_TAS[0]) + SH_TAS[1]*(P(4,5)*SH_TAS[2] + P(5,5)*SH_TAS[1] - P(22,5)*SH_TAS[2] - P(23,5)*SH_TAS[1] + P(6,5)*vd*SH_TAS[0]) - SH_TAS[2]*(P(4,22)*SH_TAS[2] + P(5,22)*SH_TAS[1] - P(22,22)*SH_TAS[2] - P(23,22)*SH_TAS[1] + P(6,22)*vd*SH_TAS[0]) - SH_TAS[1]*(P(4,23)*SH_TAS[2] + P(5,23)*SH_TAS[1] - P(22,23)*SH_TAS[2] - P(23,23)*SH_TAS[1] + P(6,23)*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P(4,6)*SH_TAS[2] + P(5,6)*SH_TAS[1] - P(22,6)*SH_TAS[2] - P(23,6)*SH_TAS[1] + P(6,6)*vd*SH_TAS[0])); + const float _airspeed_innov_var_temp = (R_TAS + SH_TAS[2]*(P(4,4)*SH_TAS[2] + P(5,4)*SH_TAS[1] - P(22,4)*SH_TAS[2] - P(23,4)*SH_TAS[1] + P(6,4)*vd*SH_TAS[0]) + SH_TAS[1]*(P(4,5)*SH_TAS[2] + P(5,5)*SH_TAS[1] - P(22,5)*SH_TAS[2] - P(23,5)*SH_TAS[1] + P(6,5)*vd*SH_TAS[0]) - SH_TAS[2]*(P(4,22)*SH_TAS[2] + P(5,22)*SH_TAS[1] - P(22,22)*SH_TAS[2] - P(23,22)*SH_TAS[1] + P(6,22)*vd*SH_TAS[0]) - SH_TAS[1]*(P(4,23)*SH_TAS[2] + P(5,23)*SH_TAS[1] - P(22,23)*SH_TAS[2] - P(23,23)*SH_TAS[1] + P(6,23)*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P(4,6)*SH_TAS[2] + P(5,6)*SH_TAS[1] - P(22,6)*SH_TAS[2] - P(23,6)*SH_TAS[1] + P(6,6)*vd*SH_TAS[0])); if (_airspeed_innov_var_temp >= R_TAS) { // Check for badly conditioned calculation SK_TAS[0] = 1.0f / _airspeed_innov_var_temp; @@ -256,7 +250,7 @@ void Ekf::resetWindStates() { // get euler yaw angle Eulerf euler321(_state.quat_nominal); - float euler_yaw = euler321(2); + const float euler_yaw = euler321(2); if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) { // estimate wind using zero sideslip assumption and airspeed measurement if airspeed available @@ -265,7 +259,6 @@ void Ekf::resetWindStates() } else { // If we don't have an airspeed measurement, then assume the wind is zero - _state.wind_vel(0) = 0.0f; - _state.wind_vel(1) = 0.0f; + _state.wind_vel.setZero(); } } diff --git a/EKF/control.cpp b/EKF/control.cpp index 0953e05e96..3854af1128 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -51,7 +51,7 @@ void Ekf::controlFusionModes() // monitor the tilt alignment if (!_control_status.flags.tilt_align) { // whilst we are aligning the tilt, monitor the variances - Vector3f angle_err_var_vec = calcRotVecVariances(); + const Vector3f angle_err_var_vec = calcRotVecVariances(); // Once the tilt variances have reduced to equivalent of 3deg uncertainty, re-set the yaw and magnetic field states // and declare the tilt alignment complete @@ -100,7 +100,7 @@ void Ekf::controlFusionModes() // if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value. // this is useful if there is a lot of interference on the sensor measurement. if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL) &&_NED_origin_initialised) { - Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0); + const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0); _mag_sample_delayed.mag(2) = calculate_synthetic_mag_z_measurement(_mag_sample_delayed.mag, mag_earth_pred); _control_status.flags.synthetic_mag_z = true; } else { @@ -228,12 +228,11 @@ void Ekf::controlExternalVisionFusion() // get initial yaw from the observation quaternion const extVisionSample &ev_newest = _ext_vision_buffer.get_newest(); - Quatf q_obs(ev_newest.quat); - Eulerf euler_obs(q_obs); + const Eulerf euler_obs(ev_newest.quat); euler_init(2) = euler_obs(2); // save a copy of the quaternion state for later use in calculating the amount of reset change - Quatf quat_before_reset = _state.quat_nominal; + const Quatf quat_before_reset = _state.quat_nominal; // calculate initial quaternion states for the ekf _state.quat_nominal = Quatf(euler_init); @@ -280,11 +279,9 @@ void Ekf::controlExternalVisionFusion() Vector2f ev_pos_innov_gates; // correct position and height for offset relative to IMU - Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; - Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - _ev_sample_delayed.pos(0) -= pos_offset_earth(0); - _ev_sample_delayed.pos(1) -= pos_offset_earth(1); - _ev_sample_delayed.pos(2) -= pos_offset_earth(2); + const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; + const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; + _ev_sample_delayed.pos -= pos_offset_earth; // Use an incremental position fusion method for EV position data if GPS is also used if (_params.fusion_mode & MASK_USE_GPS) { @@ -361,10 +358,10 @@ void Ekf::controlExternalVisionFusion() } // correct velocity for offset relative to IMU - Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt); - Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; - Vector3f vel_offset_body = ang_rate % pos_offset_body; - Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; + const Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt); + const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; + const Vector3f vel_offset_body = ang_rate % pos_offset_body; + const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; vel_aligned -= vel_offset_earth; _ev_vel_innov(0) = _state.vel(0) - vel_aligned(0); @@ -697,20 +694,19 @@ void Ekf::controlGpsFusion() Vector3f gps_pos_obs_var; // correct velocity for offset relative to IMU - 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 = ang_rate % pos_offset_body; - Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; + const Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt); + const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; + const Vector3f vel_offset_body = ang_rate % pos_offset_body; + const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; _gps_sample_delayed.vel -= vel_offset_earth; // correct position and height for offset relative to IMU - Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; + const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; _gps_sample_delayed.pos(0) -= pos_offset_earth(0); _gps_sample_delayed.pos(1) -= pos_offset_earth(1); _gps_sample_delayed.hgt += pos_offset_earth(2); - // calculate observation process noise - float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); + const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); if (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel) { // if we are using other sources of aiding, then relax the upper observation @@ -805,13 +801,13 @@ void Ekf::controlHeightSensorTimeouts() if (_control_status.flags.baro_hgt) { // check if GPS height is available const gpsSample &gps_init = _gps_buffer.get_newest(); - bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); + const bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); 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 - bool prev_bad_vert_accel = (_time_last_imu - _time_bad_vert_accel < BADACC_PROBATION); + const bool prev_bad_vert_accel = (_time_last_imu - _time_bad_vert_accel < BADACC_PROBATION); // reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data bool reset_to_gps = !_gps_hgt_intermittent && gps_hgt_accurate && !prev_bad_vert_accel; @@ -856,13 +852,13 @@ void Ekf::controlHeightSensorTimeouts() if (_control_status.flags.gps_hgt) { // check if GPS height is available const gpsSample &gps_init = _gps_buffer.get_newest(); - bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); + const 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(); - 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); + const bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); + const float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset); + const bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9,9)) * sq(_params.baro_innov_gate); // if baro data is acceptable and GPS data is inaccurate, reset height to baro bool reset_to_baro = baro_data_consistent && baro_data_fresh && !_baro_hgt_faulty && !gps_hgt_accurate; @@ -904,7 +900,7 @@ void Ekf::controlHeightSensorTimeouts() // check if baro data is available const baroSample &baro_init = _baro_buffer.get_newest(); - bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); + const 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 bool reset_to_baro = !_rng_hgt_valid && baro_data_available; @@ -940,11 +936,11 @@ void Ekf::controlHeightSensorTimeouts() if (_control_status.flags.ev_hgt) { // check if vision data is available const extVisionSample &ev_init = _ext_vision_buffer.get_newest(); - bool ev_data_available = ((_time_last_imu - ev_init.time_us) < 2 * EV_MAX_INTERVAL); + const 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(); - bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); + const 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 bool reset_to_baro = !ev_data_available && baro_data_available; @@ -1209,8 +1205,8 @@ void Ekf::controlHeightFusion() _gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset; // observation variance - receiver defined and parameter limited // use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP - float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); - float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); + const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); + const float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); gps_hgt_obs_var(2) = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit)); // innovation gate size gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f); @@ -1289,8 +1285,8 @@ void Ekf::controlAirDataFusion() // control activation and initialisation/reset of wind states required for airspeed fusion // 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 airspeed_timed_out = ((_time_last_imu - _time_last_arsp_fuse) > (uint64_t)10e6); - bool sideslip_timed_out = ((_time_last_imu - _time_last_beta_fuse) > (uint64_t)10e6); + const bool airspeed_timed_out = ((_time_last_imu - _time_last_arsp_fuse) > (uint64_t)10e6); + const bool sideslip_timed_out = ((_time_last_imu - _time_last_beta_fuse) > (uint64_t)10e6); if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) { _control_status.flags.wind = false; @@ -1332,8 +1328,8 @@ void Ekf::controlBetaFusion() // control activation and initialisation/reset of wind states required for synthetic sideslip fusion fusion // 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); + const bool sideslip_timed_out = ((_time_last_imu - _time_last_beta_fuse) > (uint64_t)10e6); + const 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)) { _control_status.flags.wind = false; diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index fbb2e65086..11a2deb051 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -53,8 +53,7 @@ void Ekf::initialiseCovariance() _delta_angle_bias_var_accum.setZero(); _delta_vel_bias_var_accum.setZero(); - // calculate average prediction time step in sec - float dt = FILTER_UPDATE_PERIOD_S; + const float dt = FILTER_UPDATE_PERIOD_S; // define the initial angle uncertainty as variances for a rotation vector Vector3f rot_vec_var; @@ -128,39 +127,39 @@ void Ekf::get_vel_var(Vector3f &vel_var) void Ekf::predictCovariance() { // assign intermediate state variables - float q0 = _state.quat_nominal(0); - float q1 = _state.quat_nominal(1); - float q2 = _state.quat_nominal(2); - float q3 = _state.quat_nominal(3); + const float q0 = _state.quat_nominal(0); + const float q1 = _state.quat_nominal(1); + const float q2 = _state.quat_nominal(2); + const float q3 = _state.quat_nominal(3); - float dax = _imu_sample_delayed.delta_ang(0); - float day = _imu_sample_delayed.delta_ang(1); - float daz = _imu_sample_delayed.delta_ang(2); + const float dax = _imu_sample_delayed.delta_ang(0); + const float day = _imu_sample_delayed.delta_ang(1); + const float daz = _imu_sample_delayed.delta_ang(2); - float dvx = _imu_sample_delayed.delta_vel(0); - float dvy = _imu_sample_delayed.delta_vel(1); - float dvz = _imu_sample_delayed.delta_vel(2); + const float dvx = _imu_sample_delayed.delta_vel(0); + const float dvy = _imu_sample_delayed.delta_vel(1); + const float dvz = _imu_sample_delayed.delta_vel(2); - float dax_b = _state.delta_ang_bias(0); - float day_b = _state.delta_ang_bias(1); - float daz_b = _state.delta_ang_bias(2); + const float dax_b = _state.delta_ang_bias(0); + const float day_b = _state.delta_ang_bias(1); + const float daz_b = _state.delta_ang_bias(2); - float dvx_b = _state.delta_vel_bias(0); - float dvy_b = _state.delta_vel_bias(1); - float dvz_b = _state.delta_vel_bias(2); + const float dvx_b = _state.delta_vel_bias(0); + const float dvy_b = _state.delta_vel_bias(1); + const float dvz_b = _state.delta_vel_bias(2); - float dt = math::constrain(_imu_sample_delayed.delta_ang_dt, 0.5f * FILTER_UPDATE_PERIOD_S, 2.0f * FILTER_UPDATE_PERIOD_S); - float dt_inv = 1.0f / dt; + const float dt = math::constrain(_imu_sample_delayed.delta_ang_dt, 0.5f * FILTER_UPDATE_PERIOD_S, 2.0f * FILTER_UPDATE_PERIOD_S); + const float dt_inv = 1.0f / dt; // convert rate of change of rate gyro bias (rad/s**2) as specified by the parameter to an expected change in delta angle (rad) since the last update - float d_ang_bias_sig = dt * dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1.0f); + const float d_ang_bias_sig = dt * dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1.0f); // convert rate of change of accelerometer bias (m/s**3) as specified by the parameter to an expected change in delta velocity (m/s) since the last update - float d_vel_bias_sig = dt * dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f); + const float d_vel_bias_sig = dt * dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f); // inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected - float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f); - float beta = 1.0f - alpha; + const float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f); + const float beta = 1.0f - alpha; _ang_rate_magnitude_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), beta * _ang_rate_magnitude_filt); _accel_magnitude_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), beta * _accel_magnitude_filt); _accel_vec_filt = alpha * dt_inv * _imu_sample_delayed.delta_vel + beta * _accel_vec_filt; diff --git a/EKF/drag_fusion.cpp b/EKF/drag_fusion.cpp index 362919e4fa..1b7f69fc08 100644 --- a/EKF/drag_fusion.cpp +++ b/EKF/drag_fusion.cpp @@ -51,30 +51,30 @@ void Ekf::fuseDrag() float Kfusion[24] = {}; // Kalman gain vector float R_ACC = _params.drag_noise; // observation noise variance in specific force drag (m/sec**2)**2 - float rho = fmaxf(_air_density, 0.1f); // air density (kg/m**3) + const float rho = fmaxf(_air_density, 0.1f); // air density (kg/m**3) // calculate inverse of ballistic coefficient 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; + const float BC_inv_x = 1.0f / _params.bcoef_x; + const float BC_inv_y = 1.0f / _params.bcoef_y; // get latest estimated orientation - float q0 = _state.quat_nominal(0); - float q1 = _state.quat_nominal(1); - float q2 = _state.quat_nominal(2); - float q3 = _state.quat_nominal(3); + const float q0 = _state.quat_nominal(0); + const float q1 = _state.quat_nominal(1); + const float q2 = _state.quat_nominal(2); + const float q3 = _state.quat_nominal(3); // get latest velocity in earth frame - float vn = _state.vel(0); - float ve = _state.vel(1); - float vd = _state.vel(2); + const float vn = _state.vel(0); + const float ve = _state.vel(1); + const float vd = _state.vel(2); // get latest wind velocity in earth frame - float vwn = _state.wind_vel(0); - float vwe = _state.wind_vel(1); + const float vwn = _state.wind_vel(0); + const float vwe = _state.wind_vel(1); // predicted specific forces // calculate relative wind velocity in earth frame and rotte into body frame @@ -82,7 +82,7 @@ void Ekf::fuseDrag() rel_wind(0) = vn - vwn; rel_wind(1) = ve - vwe; rel_wind(2) = vd; - Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal); + const Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal); rel_wind = earth_to_body * rel_wind; // perform sequential fusion of XY specific forces @@ -90,12 +90,12 @@ void Ekf::fuseDrag() // calculate observation jacobiam and Kalman gain vectors if (axis_index == 0) { // Estimate the airspeed from the measured drag force and ballistic coefficient - float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.delta_vel_bias(axis_index) / _dt_ekf_avg; - float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_x * rho)); + const float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.delta_vel_bias(axis_index) / _dt_ekf_avg; + const float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_x * rho)); // Estimate the derivative of specific force wrt airspeed along the X axis // Limit lower value to prevent arithmetic exceptions - float Kacc = fmaxf(1e-1f, rho * BC_inv_x * airSpd); + const float Kacc = fmaxf(1e-1f, rho * BC_inv_x * airSpd); SH_ACC[0] = sq(q0) + sq(q1) - sq(q2) - sq(q3); SH_ACC[1] = vn - vwn; @@ -157,18 +157,18 @@ void Ekf::fuseDrag() drag_sign = -1.0f; } - float predAccel = -BC_inv_x * 0.5f * rho * sq(rel_wind(axis_index)) * drag_sign; + const float predAccel = -BC_inv_x * 0.5f * rho * sq(rel_wind(axis_index)) * drag_sign; _drag_innov[axis_index] = predAccel - mea_acc; _drag_test_ratio[axis_index] = sq(_drag_innov[axis_index]) / (25.0f * _drag_innov_var[axis_index]); } else if (axis_index == 1) { // Estimate the airspeed from the measured drag force and ballistic coefficient - float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.delta_vel_bias(axis_index) / _dt_ekf_avg; - float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_y * rho)); + const float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.delta_vel_bias(axis_index) / _dt_ekf_avg; + const float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_y * rho)); // Estimate the derivative of specific force wrt airspeed along the X axis // Limit lower value to prevent arithmetic exceptions - float Kacc = fmaxf(1e-1f, rho * BC_inv_y * airSpd); + const float Kacc = fmaxf(1e-1f, rho * BC_inv_y * airSpd); SH_ACC[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3); SH_ACC[1] = vn - vwn; @@ -232,7 +232,7 @@ void Ekf::fuseDrag() drag_sign = -1.0f; } - float predAccel = -BC_inv_y * 0.5f * rho * sq(rel_wind(axis_index)) * drag_sign; + const float predAccel = -BC_inv_y * 0.5f * rho * sq(rel_wind(axis_index)) * drag_sign; _drag_innov[axis_index] = predAccel - mea_acc; _drag_test_ratio[axis_index] = sq(_drag_innov[axis_index]) / (25.0f * _drag_innov_var[axis_index]); diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index cfac87168a..8adf386180 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -250,7 +250,7 @@ void Ekf::predictState() { // apply imu bias corrections Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _state.delta_ang_bias; - Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.delta_vel_bias; + const Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.delta_vel_bias; // correct delta angles for earth rotation rate corrected_delta_ang -= -_R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt; @@ -266,13 +266,13 @@ void Ekf::predictState() _state.quat_nominal.normalize(); // save the previous value of velocity so we can use trapzoidal integration - Vector3f vel_last = _state.vel; + const Vector3f vel_last = _state.vel; // update transformation matrix from body to world frame _R_to_earth = Dcmf(_state.quat_nominal); // Calculate an earth frame delta velocity - Vector3f corrected_delta_vel_ef = _R_to_earth * corrected_delta_vel; + const Vector3f corrected_delta_vel_ef = _R_to_earth * corrected_delta_vel; // calculate a filtered horizontal acceleration with a 1 sec time constant // this are used for manoeuvre detection elsewhere diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 833fcf8280..2382827e41 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -391,25 +391,26 @@ void Ekf::alignOutputFilter() // It is used to align the yaw angle after launch or takeoff for fixed wing vehicle only. 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))); + const float gpsSpeed = sqrtf(sq(_gps_sample_delayed.vel(0)) + sq(_gps_sample_delayed.vel(1))); + // Need at least 5 m/s of GPS horizontal speed and + // ratio of velocity error to velocity < 0.15 for a reliable alignment if ((gpsSpeed > 5.0f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed))) { // check for excessive horizontal GPS velocity innovations - bool badVelInnov = (_gps_vel_test_ratio(0) > 1.0f) && _control_status.flags.gps; + const bool badVelInnov = (_gps_vel_test_ratio(0) > 1.0f) && _control_status.flags.gps; // calculate GPS course over ground angle - float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0)); + const float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0)); // calculate course yaw angle - float ekfGOG = atan2f(_state.vel(1), _state.vel(0)); + const float ekfGOG = atan2f(_state.vel(1), _state.vel(0)); // Check the EKF and GPS course over ground for consistency - float courseYawError = gpsCOG - ekfGOG; + const float courseYawError = gpsCOG - ekfGOG; // If the angles disagree and horizontal GPS velocity innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad - bool badYawErr = fabsf(courseYawError) > 0.5f; - bool badMagYaw = (badYawErr && badVelInnov); + const bool badYawErr = fabsf(courseYawError) > 0.5f; + const bool badMagYaw = (badYawErr && badVelInnov); if (badMagYaw) { _num_bad_flight_yaw_events ++; @@ -426,7 +427,7 @@ bool Ekf::realignYawGPS() } // save a copy of the quaternion state for later use in calculating the amount of reset change - Quatf quat_before_reset = _state.quat_nominal; + const Quatf quat_before_reset = _state.quat_nominal; // update transformation matrix from body to world frame using the current state estimate _R_to_earth = Dcmf(_state.quat_nominal); @@ -455,14 +456,12 @@ bool Ekf::realignYawGPS() // calculate new filter quaternion states using corrected yaw angle _state.quat_nominal = Quatf(euler321); + _R_to_earth = Dcmf(_state.quat_nominal); uncorrelateQuatStates(); // If heading was bad, then we also need to reset the velocity and position states _velpos_reset_request = badMagYaw; - // update transformation matrix from body to world frame using the current state estimate - _R_to_earth = Dcmf(_state.quat_nominal); - // Use the last magnetometer measurements to reset the field states _state.mag_B.zero(); _state.mag_I = _R_to_earth * _mag_sample_delayed.mag; @@ -551,7 +550,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool } // save a copy of the quaternion state for later use in calculating the amount of reset change - Quatf quat_before_reset = _state.quat_nominal; + const Quatf quat_before_reset = _state.quat_nominal; Quatf quat_after_reset = _state.quat_nominal; // update transformation matrix from body to world frame using the current estimate @@ -572,13 +571,13 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool // calculate the observed yaw angle if (_control_status.flags.ev_yaw) { // convert the observed quaternion to a rotation matrix - Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame + const Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame // calculate the yaw angle for a 312 sequence euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0)); } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { // rotate the magnetometer measurements into earth frame using a zero yaw angle - Vector3f mag_earth_pred = R_to_earth * mag_init; + const Vector3f mag_earth_pred = R_to_earth * mag_init; // the angle of the projection onto the horizontal gives the yaw angle euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); @@ -630,13 +629,13 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool // calculate the observed yaw angle if (_control_status.flags.ev_yaw) { // convert the observed quaternion to a rotation matrix - Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame + const Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame // calculate the yaw angle for a 312 sequence euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1)); } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { // rotate the magnetometer measurements into earth frame using a zero yaw angle - Vector3f mag_earth_pred = R_to_earth * mag_init; + const Vector3f mag_earth_pred = R_to_earth * mag_init; // the angle of the projection onto the horizontal gives the yaw angle euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); @@ -668,7 +667,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool } // set the earth magnetic field states using the updated rotation - Dcmf R_to_earth_after(quat_after_reset); + const Dcmf R_to_earth_after(quat_after_reset); _state.mag_I = R_to_earth_after * mag_init; // reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance @@ -1198,21 +1197,21 @@ hagl_max : Maximum height above ground (meters). NaN when limiting is not needed void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) { // Calculate range finder limits - float rangefinder_hagl_min = _rng_valid_min_val; + const float rangefinder_hagl_min = _rng_valid_min_val; // Allow use of 75% of rangefinder maximum range to allow for angular motion - float rangefinder_hagl_max = 0.75f * _rng_valid_max_val; + const float rangefinder_hagl_max = 0.75f * _rng_valid_max_val; // Calculate optical flow limits // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion - float flow_vxy_max = fmaxf(0.5f * _flow_max_rate * (_terrain_vpos - _state.pos(2)), 0.0f); - float flow_hagl_min = _flow_min_distance; - float flow_hagl_max = _flow_max_distance; + const float flow_vxy_max = fmaxf(0.5f * _flow_max_rate * (_terrain_vpos - _state.pos(2)), 0.0f); + const float flow_hagl_min = _flow_min_distance; + const float flow_hagl_max = _flow_max_distance; // TODO : calculate visual odometry limits - bool relying_on_rangefinder = _control_status.flags.rng_hgt && !_params.range_aid; + const bool relying_on_rangefinder = _control_status.flags.rng_hgt && !_params.range_aid; - bool relying_on_optical_flow = _control_status.flags.opt_flow && !(_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel); + const bool relying_on_optical_flow = _control_status.flags.opt_flow && !(_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel); // Do not require limiting by default *vxy_max = NAN; @@ -1649,8 +1648,8 @@ void Ekf::calcExtVisRotMat() 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_len = _input_delta_vec.norm(); + const Vector3f _input_delta_vec = rot_vec - _ev_rot_vec_filt; + const float input_delta_len = _input_delta_vec.norm(); if (input_delta_len > 0.1f) { rot_vec = _ev_rot_vec_filt + _input_delta_vec * (0.1f / input_delta_len); diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index b472727021..f2db9dc7c6 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -385,10 +385,9 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl flow_magnitude_good = (flow_rate_magnitude <= _flow_max_rate); } - bool relying_on_flow = !_control_status.flags.gps && !_control_status.flags.ev_pos && !_control_status.flags.ev_vel; + const bool relying_on_flow = !_control_status.flags.gps && !_control_status.flags.ev_pos && !_control_status.flags.ev_vel; - // check quality metric - bool flow_quality_good = (flow->quality >= _params.flow_qual_min); + const bool flow_quality_good = (flow->quality >= _params.flow_qual_min); // Check data validity and write to buffers // Invalid flow data is allowed when on ground and is handled as a special case in controlOpticalFlowFusion() @@ -507,7 +506,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) // set the observation buffer length to handle the minimum time of arrival between observations in combination // with the worst case delay from current time to ekf fusion time // allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter - uint16_t ekf_delay_ms = max_time_delay_ms + (int)(ceilf((float)max_time_delay_ms * 0.5f)); + const uint16_t ekf_delay_ms = max_time_delay_ms + (int)(ceilf((float)max_time_delay_ms * 0.5f)); _obs_buffer_length = (ekf_delay_ms / _params.sensor_interval_min_ms) + 1; // limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate) diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index ea8931d939..b72ec3b60a 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -62,8 +62,8 @@ bool Ekf::collect_gps(const gps_message &gps) _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; - double lon = gps.lon / 1.0e7; + const double lat = gps.lat / 1.0e7; + const double lon = gps.lon / 1.0e7; map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu); // if we are already doing aiding, correct for the change in position since the EKF started navigationg @@ -137,12 +137,12 @@ bool Ekf::gps_is_good(const gps_message &gps) // Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient const float filt_time_const = 10.0f; - float dt = fminf(fmaxf(float(_time_last_imu - _gps_pos_prev.timestamp) * 1e-6f, 0.001f), filt_time_const); - float filter_coef = dt / filt_time_const; + const float dt = fminf(fmaxf(float(_time_last_imu - _gps_pos_prev.timestamp) * 1e-6f, 0.001f), filt_time_const); + const float filter_coef = dt / filt_time_const; // 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; + const double lat = gps.lat * 1.0e-7; + const double lon = gps.lon * 1.0e-7; if (!_control_status.flags.in_air && _vehicle_at_rest) { // Calculate position movement since last measurement float delta_pos_n = 0.0f; diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index 68d1b18337..984cc42800 100644 --- a/EKF/gps_yaw_fusion.cpp +++ b/EKF/gps_yaw_fusion.cpp @@ -48,10 +48,10 @@ void Ekf::fuseGpsAntYaw() { // assign intermediate state variables - float q0 = _state.quat_nominal(0); - float q1 = _state.quat_nominal(1); - float q2 = _state.quat_nominal(2); - float q3 = _state.quat_nominal(3); + const float q0 = _state.quat_nominal(0); + const float q1 = _state.quat_nominal(1); + const float q2 = _state.quat_nominal(2); + const float q3 = _state.quat_nominal(3); float R_YAW = 1.0f; float predicted_hdg; @@ -295,24 +295,24 @@ bool Ekf::resetGpsAntYaw() if (ISFINITE(_gps_sample_delayed.yaw)) { // define the predicted antenna array vector and rotate into earth frame - Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f}; - Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf; + const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f}; + const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf; // check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) { return false; } - float predicted_yaw = atan2f(ant_vec_ef(1),ant_vec_ef(0)); + const 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; + const float measured_yaw = _gps_sample_delayed.yaw + _gps_yaw_offset; // calculate the amount the yaw needs to be rotated by float yaw_delta = wrap_pi(measured_yaw - predicted_yaw); // save a copy of the quaternion state for later use in calculating the amount of reset change - Quatf quat_before_reset = _state.quat_nominal; + const Quatf quat_before_reset = _state.quat_nominal; Quatf quat_after_reset = _state.quat_nominal; // obtain the yaw angle using the best conditioned from either a Tait-Bryan 321 or 312 sequence diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 463ff61ba8..dffbae595a 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -47,18 +47,17 @@ void Ekf::fuseMag() { // assign intermediate variables - float q0 = _state.quat_nominal(0); - float q1 = _state.quat_nominal(1); - float q2 = _state.quat_nominal(2); - float q3 = _state.quat_nominal(3); + const float q0 = _state.quat_nominal(0); + const float q1 = _state.quat_nominal(1); + const float q2 = _state.quat_nominal(2); + const float q3 = _state.quat_nominal(3); - float magN = _state.mag_I(0); - float magE = _state.mag_I(1); - float magD = _state.mag_I(2); + const float magN = _state.mag_I(0); + const float magE = _state.mag_I(1); + const float magD = _state.mag_I(2); // XYZ Measurement uncertainty. Need to consider timing errors for fast rotations - float R_MAG = fmaxf(_params.mag_noise, 0.0f); - R_MAG = R_MAG * R_MAG; + const float R_MAG = sq(fmaxf(_params.mag_noise, 0.0f)); // intermediate variables from algebraic optimisation float SH_MAG[9]; @@ -73,9 +72,9 @@ void Ekf::fuseMag() SH_MAG[8] = 2.0f*magE*q3; // rotate magnetometer earth field state into body frame - Dcmf R_to_body = quat_to_invrotmat(_state.quat_nominal); + const Dcmf R_to_body = quat_to_invrotmat(_state.quat_nominal); - Vector3f mag_I_rot = R_to_body * _state.mag_I; + const Vector3f mag_I_rot = R_to_body * _state.mag_I; // compute magnetometer innovations _mag_innov[0] = (mag_I_rot(0) + _state.mag_B(0)) - _mag_sample_delayed.mag(0); @@ -437,10 +436,10 @@ void Ekf::fuseMag() void Ekf::fuseHeading() { // assign intermediate state variables - float q0 = _state.quat_nominal(0); - float q1 = _state.quat_nominal(1); - float q2 = _state.quat_nominal(2); - float q3 = _state.quat_nominal(3); + const float q0 = _state.quat_nominal(0); + const float q1 = _state.quat_nominal(1); + const float q2 = _state.quat_nominal(2); + const float q3 = _state.quat_nominal(3); float R_YAW = 1.0f; float predicted_hdg; @@ -488,7 +487,7 @@ void Ekf::fuseHeading() if (_control_status.flags.mag_hdg) { // Set the yaw angle to zero and rotate the measurements into earth frame using the zero yaw angle euler321(2) = 0.0f; - Dcmf R_to_earth(euler321); + const Dcmf R_to_earth(euler321); // rotate the magnetometer measurements into earth frame using a zero yaw angle if (_control_status.flags.mag_3D) { @@ -505,8 +504,8 @@ void Ekf::fuseHeading() } else if (_control_status.flags.ev_yaw) { // calculate the yaw angle for a 321 sequence // Expressions obtained from yaw_input_321.c produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw321.m - float Tbn_1_0 = 2.0f*(_ev_sample_delayed.quat(0)*_ev_sample_delayed.quat(3)+_ev_sample_delayed.quat(1)*_ev_sample_delayed.quat(2)); - float Tbn_0_0 = sq(_ev_sample_delayed.quat(0))+sq(_ev_sample_delayed.quat(1))-sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3)); + const float Tbn_1_0 = 2.0f*(_ev_sample_delayed.quat(0)*_ev_sample_delayed.quat(3)+_ev_sample_delayed.quat(1)*_ev_sample_delayed.quat(2)); + const float Tbn_0_0 = sq(_ev_sample_delayed.quat(0))+sq(_ev_sample_delayed.quat(1))-sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3)); measured_hdg = atan2f(Tbn_1_0,Tbn_0_0); } else if (_mag_use_inhibit) { @@ -561,8 +560,8 @@ void Ekf::fuseHeading() [ -cos(roll)*sin(pitch), sin(roll), cos(pitch)*cos(roll)] */ float yaw = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)); // first rotation (yaw) - float roll = asinf(_R_to_earth(2, 1)); // second rotation (roll) - float pitch = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch) + const float roll = asinf(_R_to_earth(2, 1)); // second rotation (roll) + const float pitch = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch) predicted_hdg = yaw; // we will need the predicted heading to calculate the innovation @@ -813,14 +812,14 @@ void Ekf::fuseHeading() void Ekf::fuseDeclination(float decl_sigma) { // assign intermediate state variables - float magN = _state.mag_I(0); - float magE = _state.mag_I(1); + const float magN = _state.mag_I(0); + const float magE = _state.mag_I(1); // minimum horizontal field strength before calculation becomes badly conditioned (T) - float h_field_min = 0.001f; + const float h_field_min = 0.001f; // observation variance (rad**2) - float R_DECL = sq(decl_sigma); + const float R_DECL = sq(decl_sigma); // Calculate intermediate variables float t2 = magE*magE; @@ -889,9 +888,7 @@ void Ekf::fuseDeclination(float decl_sigma) Kfusion[22] = -t4*t13*(P(22,16)*magE-P(22,17)*magN); Kfusion[23] = -t4*t13*(P(23,16)*magE-P(23,17)*magN); - // calculate innovation and constrain - float innovation = atan2f(magE, magN) - getMagDeclination(); - innovation = math::constrain(innovation, -0.5f, 0.5f); + const float innovation = math::constrain(atan2f(magE, magN) - getMagDeclination(), -0.5f, 0.5f); // apply covariance correction via P_new = (I -K*H)*P // first calculate expression for KHP @@ -1004,7 +1001,7 @@ float Ekf::calculate_synthetic_mag_z_measurement(Vector3f mag_meas, Vector3f mag { // theoretical magnitude of the magnetometer Z component value given X and Y sensor measurement and our knowledge // of the earth magnetic field vector at the current location - float mag_z_abs = sqrtf(math::max(sq(mag_earth_predicted.length()) - sq(mag_meas(0)) - sq(mag_meas(1)), 0.0f)); + const float mag_z_abs = sqrtf(math::max(sq(mag_earth_predicted.length()) - sq(mag_meas(0)) - sq(mag_meas(1)), 0.0f)); // calculate sign of synthetic magnetomter Z component based on the sign of the predicted magnetomer Z component float mag_z_body_pred = _R_to_earth(0,2) * mag_earth_predicted(0) + _R_to_earth(1,2) * mag_earth_predicted(1) + _R_to_earth(2,2) * mag_earth_predicted(2); diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index a91d17ba3f..a84129f697 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -50,43 +50,43 @@ void Ekf::fuseOptFlow() float gndclearance = fmaxf(_params.rng_gnd_clearance, 0.1f); // get latest estimated orientation - float q0 = _state.quat_nominal(0); - float q1 = _state.quat_nominal(1); - float q2 = _state.quat_nominal(2); - float q3 = _state.quat_nominal(3); + const float q0 = _state.quat_nominal(0); + const float q1 = _state.quat_nominal(1); + const float q2 = _state.quat_nominal(2); + const float q3 = _state.quat_nominal(3); // get latest velocity in earth frame - float vn = _state.vel(0); - float ve = _state.vel(1); - float vd = _state.vel(2); + const float vn = _state.vel(0); + const float ve = _state.vel(1); + const float vd = _state.vel(2); // calculate the optical flow observation variance - float R_LOS = calcOptFlowMeasVar(); + const float R_LOS = calcOptFlowMeasVar(); float H_LOS[2][24] = {}; // Optical flow observation Jacobians float Kfusion[24][2] = {}; // Optical flow Kalman gains // get rotation matrix from earth to body - Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal); + const Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal); // calculate the sensor position relative to the IMU - Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body; + const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body; // calculate the velocity of the sensor relative to the imu in body frame // Note: _flow_sample_delayed.gyroXYZ is the negative of the body angular velocity, thus use minus sign - Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt) % pos_offset_body; + const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt) % pos_offset_body; // calculate the velocity of the sensor in the earth frame - Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body; + const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body; // rotate into body frame - Vector3f vel_body = earth_to_body * vel_rel_earth; + const Vector3f vel_body = earth_to_body * vel_rel_earth; // height above ground of the IMU float heightAboveGndEst = _terrain_vpos - _state.pos(2); // calculate the sensor position relative to the IMU in earth frame - Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; + const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; // calculate the height above the ground of the optical flow camera. Since earth frame is NED // a positive offset in earth frame leads to a smaller height above the ground. @@ -96,7 +96,7 @@ void Ekf::fuseOptFlow() heightAboveGndEst = math::max(heightAboveGndEst, gndclearance); // calculate range from focal point to centre of image - float range = heightAboveGndEst / earth_to_body(2, 2); // absolute distance to the frame region in view + const float range = heightAboveGndEst / earth_to_body(2, 2); // absolute distance to the frame region in view // calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed // correct for gyro bias errors in the data used to do the motion compensation @@ -522,20 +522,17 @@ 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)); + const 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) { // if accumulation time differences are not excessive and accumulation time is adequate // compare the optical flow and and navigation rate data and calculate a bias error if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) && (_delta_time_of > FLT_EPSILON)) { - // calculate a reference angular rate - Vector3f reference_body_rate; - reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of); - // calculate the optical flow sensor measured body rate - Vector3f of_body_rate; - of_body_rate = _flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt); + const Vector3f reference_body_rate(_imu_del_ang_of * (1.0f / _delta_time_of)); + + const Vector3f measured_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)), @@ -563,8 +560,8 @@ bool Ekf::calcOptFlowBodyRateComp() float Ekf::calcOptFlowMeasVar() { // calculate the observation noise variance - scaling noise linearly across flow quality range - float R_LOS_best = fmaxf(_params.flow_noise, 0.05f); - float R_LOS_worst = fmaxf(_params.flow_noise_qual_min, 0.05f); + const float R_LOS_best = fmaxf(_params.flow_noise, 0.05f); + const float R_LOS_worst = fmaxf(_params.flow_noise_qual_min, 0.05f); // calculate a weighting that varies between 1 when flow quality is best and 0 when flow quality is worst float weighting = (255.0f - (float)_params.flow_qual_min); @@ -578,7 +575,7 @@ float Ekf::calcOptFlowMeasVar() } // take the weighted average of the observation noise for the best and wort flow quality - float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.0f - weighting)); + const float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.0f - weighting)); return R_LOS; } diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index a67ca17856..8672f0529b 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -53,19 +53,19 @@ void Ekf::fuseSideslip() float R_BETA = _params.beta_noise; // get latest estimated orientation - float q0 = _state.quat_nominal(0); - float q1 = _state.quat_nominal(1); - float q2 = _state.quat_nominal(2); - float q3 = _state.quat_nominal(3); + const float q0 = _state.quat_nominal(0); + const float q1 = _state.quat_nominal(1); + const float q2 = _state.quat_nominal(2); + const float q3 = _state.quat_nominal(3); // get latest velocity in earth frame - float vn = _state.vel(0); - float ve = _state.vel(1); - float vd = _state.vel(2); + const float vn = _state.vel(0); + const float ve = _state.vel(1); + const float vd = _state.vel(2); // get latest wind velocity in earth frame - float vwn = _state.wind_vel(0); - float vwe = _state.wind_vel(1); + const float vwn = _state.wind_vel(0); + const float vwe = _state.wind_vel(1); // relative wind velocity in earth frame Vector3f rel_wind; @@ -73,7 +73,7 @@ void Ekf::fuseSideslip() rel_wind(1) = ve - vwe; rel_wind(2) = vd; - Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal); + const Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal); // rotate into body axes rel_wind = earth_to_body * rel_wind; diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 061ccaa1d7..e0c75d1abc 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -135,16 +135,16 @@ void Ekf::fuseHagl() // If the vehicle is excessively tilted, do not try to fuse range finder observations if (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt) { // get a height above ground measurement from the range finder assuming a flat earth - float meas_hagl = _range_sample_delayed.rng * _R_rng_to_earth_2_2; + const float meas_hagl = _range_sample_delayed.rng * _R_rng_to_earth_2_2; // predict the hagl from the vehicle position and terrain height - float pred_hagl = _terrain_vpos - _state.pos(2); + const float pred_hagl = _terrain_vpos - _state.pos(2); // calculate the innovation _hagl_innov = pred_hagl - meas_hagl; // calculate the observation variance adding the variance of the vehicles own height uncertainty - float obs_variance = fmaxf(P(9,9) * _params.vehicle_variance_scaler, 0.0f) + const float obs_variance = fmaxf(P(9,9) * _params.vehicle_variance_scaler, 0.0f) + sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng); @@ -152,7 +152,7 @@ void Ekf::fuseHagl() _hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); // perform an innovation consistency check and only fuse data if it passes - float gate_size = fmaxf(_params.range_innov_gate, 1.0f); + const float gate_size = fmaxf(_params.range_innov_gate, 1.0f); _hagl_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var); if (_hagl_test_ratio <= 1.0f) { @@ -190,38 +190,38 @@ void Ekf::fuseFlowForTerrain() const Vector2f opt_flow_rate = Vector2f{_flowRadXYcomp} / _flow_sample_delayed.dt + Vector2f{_flow_gyro_bias}; // get latest estimated orientation - float q0 = _state.quat_nominal(0); - float q1 = _state.quat_nominal(1); - float q2 = _state.quat_nominal(2); - float q3 = _state.quat_nominal(3); + const float q0 = _state.quat_nominal(0); + const float q1 = _state.quat_nominal(1); + const float q2 = _state.quat_nominal(2); + const float q3 = _state.quat_nominal(3); // calculate the optical flow observation variance - float R_LOS = calcOptFlowMeasVar(); + const float R_LOS = calcOptFlowMeasVar(); // get rotation matrix from earth to body - Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal); + const Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal); // calculate the sensor position relative to the IMU - Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body; + const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body; // calculate the velocity of the sensor relative to the imu in body frame // Note: _flow_sample_delayed.gyroXYZ is the negative of the body angular velocity, thus use minus sign - Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt) % pos_offset_body; + const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt) % pos_offset_body; // calculate the velocity of the sensor in the earth frame - Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body; + const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body; // rotate into body frame - Vector3f vel_body = earth_to_body * vel_rel_earth; + const Vector3f vel_body = earth_to_body * vel_rel_earth; - float t0 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; + const float t0 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; // constrain terrain to minimum allowed value and predict height above ground _terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2)); - float pred_hagl = _terrain_vpos - _state.pos(2); + const 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); + const float Hx = vel_body(1) * t0 / (pred_hagl * pred_hagl); // Constrain terrain variance to be non-negative _terrain_var = fmaxf(_terrain_var, 0.0f); @@ -230,19 +230,19 @@ void Ekf::fuseFlowForTerrain() _flow_innov_var[0] = Hx * Hx * _terrain_var + R_LOS; // calculate the kalman gain for the flow x measurement - float Kx = _terrain_var * Hx / _flow_innov_var[0]; + const 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; + const 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); // calculate correction term for terrain variance - float KxHxP = Kx * Hx * _terrain_var; + const float KxHxP = Kx * Hx * _terrain_var; // innovation consistency check - float gate_size = fmaxf(_params.flow_innov_gate, 1.0f); + const float gate_size = fmaxf(_params.flow_innov_gate, 1.0f); float flow_test_ratio = sq(_flow_innov[0]) / (sq(gate_size) * _flow_innov_var[0]); // do not perform measurement update if badly conditioned @@ -254,22 +254,22 @@ void Ekf::fuseFlowForTerrain() } // Calculate observation matrix for flow around the vehicle y axis - float Hy = -vel_body(0) * t0 / (pred_hagl * pred_hagl); + const float Hy = -vel_body(0) * t0 / (pred_hagl * pred_hagl); // Calculuate innovation variance _flow_innov_var[1] = Hy * Hy * _terrain_var + R_LOS; // calculate the kalman gain for the flow y measurement - float Ky = _terrain_var * Hy / _flow_innov_var[1]; + const 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; + const 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); // calculate correction term for terrain variance - float KyHyP = Ky * Hy * _terrain_var; + const float KyHyP = Ky * Hy * _terrain_var; // innovation consistency check flow_test_ratio = sq(_flow_innov[1]) / (sq(gate_size) * _flow_innov_var[1]); @@ -290,11 +290,11 @@ bool Ekf::isTerrainEstimateValid() const void Ekf::updateTerrainValidity() { // we have been fusing range finder measurements in the last 5 seconds - bool recent_range_fusion = (_time_last_imu - _time_last_hagl_fuse) < (uint64_t)5e6; + const bool recent_range_fusion = (_time_last_imu - _time_last_hagl_fuse) < (uint64_t)5e6; // we have been fusing optical flow measurements for terrain estimation within the last 5 seconds // this can only be the case if the main filter does not fuse optical flow - bool recent_flow_for_terrain_fusion = ((_time_last_imu - _time_last_of_fuse) < (uint64_t)5e6) + const bool recent_flow_for_terrain_fusion = ((_time_last_imu - _time_last_of_fuse) < (uint64_t)5e6) && !_control_status.flags.opt_flow; _hagl_valid = (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion)); diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 336d0430f4..4305a67233 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -54,7 +54,7 @@ bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_ga test_ratio(0) = fmaxf( sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)), sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1))); - bool innov_check_pass = (test_ratio(0) <= 1.0f); + const bool innov_check_pass = (test_ratio(0) <= 1.0f); if (innov_check_pass) { _time_last_hor_vel_fuse = _time_last_imu; @@ -76,7 +76,7 @@ bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate innov_var(2) = P(6,6) + obs_var(2); test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2)); - bool innov_check_pass = (test_ratio(1) <= 1.0f); + const bool innov_check_pass = (test_ratio(1) <= 1.0f); if (innov_check_pass) { _time_last_ver_vel_fuse = _time_last_imu; _innov_check_fail_status.flags.reject_ver_vel = false; @@ -124,7 +124,7 @@ bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate innov_var(2) = P(9,9) + obs_var(2); test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2)); - bool innov_check_pass = (test_ratio(1) <= 1.0f) || !_control_status.flags.tilt_align; + const bool innov_check_pass = (test_ratio(1) <= 1.0f) || !_control_status.flags.tilt_align; if (innov_check_pass) { _time_last_hgt_fuse = _time_last_imu; _innov_check_fail_status.flags.reject_ver_pos = false; @@ -142,7 +142,7 @@ bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index) { float Kfusion[24] = {}; // Kalman gain vector for any single observation - sequential fusion is used. - unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state + const unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state // calculate kalman gain K = PHS, where S = 1/innovation variance for (int row = 0; row < _k_num_states; row++) {