mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-14 05:33:34 +08:00
Add const modifier
const modifier Add missing const modifier
This commit is contained in:
kamilritz
committed by
Mathieu Bresciani
Mathieu Bresciani
parent
e357376797
commit
b5e1397c0f
+11
-16
@@ -236,10 +236,10 @@ void EKFGSF_yaw::ahrsAlignYaw()
|
||||
|
||||
} else {
|
||||
// Calculate the 312 Tait-Bryan rotation sequence that rotates from earth to body frame
|
||||
Vector3f rot312;
|
||||
rot312(0) = wrap_pi(_ekf_gsf[model_index].X(2)); // first rotation (yaw) taken from EKF model state
|
||||
rot312(1) = asinf(_ahrs_ekf_gsf[model_index].R(2, 1)); // second rotation (roll)
|
||||
rot312(2) = atan2f(-_ahrs_ekf_gsf[model_index].R(2, 0), _ahrs_ekf_gsf[model_index].R(2, 2)); // third rotation (pitch)
|
||||
const Vector3f rot312(wrap_pi(_ekf_gsf[model_index].X(2)), // yaw
|
||||
asinf(_ahrs_ekf_gsf[model_index].R(2, 1)), // roll
|
||||
atan2f(-_ahrs_ekf_gsf[model_index].R(2, 0),
|
||||
_ahrs_ekf_gsf[model_index].R(2, 2))); // pitch
|
||||
|
||||
// Calculate the body to earth frame rotation matrix
|
||||
_ahrs_ekf_gsf[model_index].R = taitBryan312ToRotMat(rot312);
|
||||
@@ -312,7 +312,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
|
||||
const float t15 = P22*t10;
|
||||
const float t16 = P12+t15;
|
||||
|
||||
const float min_var = 1e-6f;
|
||||
constexpr float min_var = 1e-6f;
|
||||
_ekf_gsf[model_index].P(0,0) = fmaxf(P00-P20*t6+dvxVar*t14+dvyVar*t13-t6*t7 , min_var);
|
||||
_ekf_gsf[model_index].P(0,1) = P01+t12-P21*t6+t7*t10-dvyVar*t2*t3;
|
||||
_ekf_gsf[model_index].P(0,2) = t7;
|
||||
@@ -356,17 +356,14 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index)
|
||||
// Update the inverse of the innovation covariance matrix S_inverse
|
||||
updateInnovCovMatInv(model_index, S);
|
||||
|
||||
// Perform a chi-square innovation consistency test and calculate a compression scale factor that limits the magnitude of innovations to 5-sigma
|
||||
float innov_comp_scale_factor = 1.0f;
|
||||
|
||||
// test ratio = transpose(innovation) * inverse(innovation variance) * innovation = [1x2] * [2,2] * [2,1] = [1,1]
|
||||
const float test_ratio = _ekf_gsf[model_index].innov * (_ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov);
|
||||
|
||||
// Perform a chi-square innovation consistency test and calculate a compression scale factor
|
||||
// that limits the magnitude of innovations to 5-sigma
|
||||
// If the test ratio is greater than 25 (5 Sigma) then reduce the length of the innovation vector to clip it at 5-Sigma
|
||||
// This protects from large measurement spikes
|
||||
if (test_ratio > 25.0f) {
|
||||
innov_comp_scale_factor = sqrtf(25.0f / test_ratio);
|
||||
}
|
||||
const float innov_comp_scale_factor = test_ratio > 25.f ? sqrtf(25.0f / test_ratio) : 1.f;
|
||||
|
||||
// calculate Kalman gain K nd covariance matrix P
|
||||
// autocode from https://github.com/priseborough/3_state_filter/blob/flightLogReplay-wip/calcK.txt
|
||||
@@ -377,13 +374,11 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index)
|
||||
const float t5 = P00*P11;
|
||||
const float t9 = P01*P10;
|
||||
const float t6 = t2+t3+t4+t5-t9;
|
||||
float t7;
|
||||
if (fabsf(t6) > 1e-6f) {
|
||||
t7 = 1.0f/t6;
|
||||
} else {
|
||||
// skip this fusion step
|
||||
if (fabsf(t6) < 1e-6f) {
|
||||
// skip the fusion step
|
||||
return false;
|
||||
}
|
||||
const float t7 = 1.f / t6;
|
||||
const float t8 = P11+velObsVar;
|
||||
const float t10 = P00+velObsVar;
|
||||
|
||||
|
||||
+5
-5
@@ -771,10 +771,10 @@ void Ekf::controlHeightSensorTimeouts()
|
||||
checkVerticalAccelerationHealth();
|
||||
|
||||
// check if height is continuously failing because of accel errors
|
||||
bool continuous_bad_accel_hgt = isTimedOut(_time_good_vert_accel, (uint64_t)_params.bad_acc_reset_delay_us);
|
||||
const bool continuous_bad_accel_hgt = isTimedOut(_time_good_vert_accel, (uint64_t)_params.bad_acc_reset_delay_us);
|
||||
|
||||
// check if height has been inertial deadreckoning for too long
|
||||
bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6);
|
||||
const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6);
|
||||
|
||||
if (hgt_fusion_timeout || continuous_bad_accel_hgt) {
|
||||
|
||||
@@ -1079,8 +1079,8 @@ void Ekf::controlHeightFusion()
|
||||
|
||||
// Compensate for positive static pressure transients (negative vertical position innovations)
|
||||
// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
|
||||
float deadzone_start = 0.0f;
|
||||
float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
|
||||
const float deadzone_start = 0.0f;
|
||||
const float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
|
||||
|
||||
if (_control_status.flags.gnd_effect) {
|
||||
if (_baro_hgt_innov(2) < -deadzone_start) {
|
||||
@@ -1225,7 +1225,7 @@ void Ekf::controlBetaFusion()
|
||||
// Perform synthetic sideslip fusion when in-air and sideslip fuson had been enabled externally in addition to the following criteria:
|
||||
|
||||
// Sufficient time has lapsed sice the last fusion
|
||||
bool beta_fusion_time_triggered = isTimedOut(_time_last_beta_fuse, _params.beta_avg_ft_us);
|
||||
const bool beta_fusion_time_triggered = isTimedOut(_time_last_beta_fuse, _params.beta_avg_ft_us);
|
||||
|
||||
if (beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air) {
|
||||
// If starting wind state estimation, reset the wind states and covariances before fusing any data
|
||||
|
||||
+1
-1
@@ -268,7 +268,7 @@ void Ekf::predictState()
|
||||
|
||||
// calculate a filtered horizontal acceleration with a 1 sec time constant
|
||||
// this are used for manoeuvre detection elsewhere
|
||||
float alpha = 1.0f - _imu_sample_delayed.delta_vel_dt;
|
||||
const float alpha = 1.0f - _imu_sample_delayed.delta_vel_dt;
|
||||
_accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy();
|
||||
|
||||
// save the previous value of velocity so we can use trapzoidal integration
|
||||
|
||||
@@ -658,9 +658,9 @@ private:
|
||||
// update the rotation matrix which transforms EV navigation frame measurements into NED
|
||||
void calcExtVisRotMat();
|
||||
|
||||
Vector3f getVisionVelocityInEkfFrame();
|
||||
Vector3f getVisionVelocityInEkfFrame() const;
|
||||
|
||||
Vector3f getVisionVelocityVarianceInEkfFrame();
|
||||
Vector3f getVisionVelocityVarianceInEkfFrame() const;
|
||||
|
||||
// limit the diagonal of the covariance matrix
|
||||
// force symmetry when the argument is true
|
||||
|
||||
+17
-18
@@ -219,7 +219,7 @@ void Ekf::resetHeight()
|
||||
const gpsSample &gps_newest = _gps_buffer.get_newest();
|
||||
|
||||
// store the current vertical position and velocity for reference so we can calculate and publish the reset amount
|
||||
float old_vert_pos = _state.pos(2);
|
||||
const float old_vert_pos = _state.pos(2);
|
||||
bool vert_pos_reset = false;
|
||||
|
||||
// reset the vertical position
|
||||
@@ -277,8 +277,8 @@ void Ekf::resetHeight()
|
||||
const extVisionSample &ev_newest = _ext_vision_buffer.get_newest();
|
||||
|
||||
// use the most recent data if it's time offset from the fusion time horizon is smaller
|
||||
int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us;
|
||||
int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us;
|
||||
const int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us;
|
||||
const int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us;
|
||||
|
||||
vert_pos_reset = true;
|
||||
|
||||
@@ -1090,9 +1090,9 @@ void Ekf::get_ekf_soln_status(uint16_t *status)
|
||||
soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz;
|
||||
soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel;
|
||||
soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs;
|
||||
bool gps_vel_innov_bad = (_gps_vel_test_ratio(0) > 1.0f) || (_gps_vel_test_ratio(1) > 1.0f);
|
||||
bool gps_pos_innov_bad = (_gps_pos_test_ratio(0) > 1.0f);
|
||||
bool mag_innov_good = (_mag_test_ratio.max() < 1.0f) && (_yaw_test_ratio < 1.0f);
|
||||
const bool gps_vel_innov_bad = (_gps_vel_test_ratio(0) > 1.0f) || (_gps_vel_test_ratio(1) > 1.0f);
|
||||
const bool gps_pos_innov_bad = (_gps_pos_test_ratio(0) > 1.0f);
|
||||
const bool mag_innov_good = (_mag_test_ratio.max() < 1.0f) && (_yaw_test_ratio < 1.0f);
|
||||
soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good;
|
||||
soln_status.flags.accel_error = _bad_vert_accel_detected;
|
||||
*status = soln_status.value;
|
||||
@@ -1127,14 +1127,14 @@ bool Ekf::global_position_is_valid()
|
||||
// return true if we are totally reliant on inertial dead-reckoning for position
|
||||
void Ekf::update_deadreckoning_status()
|
||||
{
|
||||
bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel)
|
||||
&& (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
|
||||
const bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel)
|
||||
&& (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
|
||||
|| isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max)
|
||||
|| isRecent(_time_last_delpos_fuse, _params.no_aid_timeout_max));
|
||||
bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_time_last_of_fuse, _params.no_aid_timeout_max);
|
||||
bool airDataAiding = _control_status.flags.wind &&
|
||||
isRecent(_time_last_arsp_fuse, _params.no_aid_timeout_max) &&
|
||||
isRecent(_time_last_beta_fuse, _params.no_aid_timeout_max);
|
||||
const bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_time_last_of_fuse, _params.no_aid_timeout_max);
|
||||
const bool airDataAiding = _control_status.flags.wind &&
|
||||
isRecent(_time_last_arsp_fuse, _params.no_aid_timeout_max) &&
|
||||
isRecent(_time_last_beta_fuse, _params.no_aid_timeout_max);
|
||||
|
||||
_is_wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding;
|
||||
_is_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding;
|
||||
@@ -1398,17 +1398,16 @@ void Ekf::updateBaroHgtOffset()
|
||||
// calculate a filtered offset between the baro origin and local NED origin if we are not
|
||||
// using the baro as a height reference
|
||||
if (!_control_status.flags.baro_hgt && _baro_data_ready) {
|
||||
float local_time_step = 1e-6f * _delta_time_baro_us;
|
||||
local_time_step = math::constrain(local_time_step, 0.0f, 1.0f);
|
||||
const float local_time_step = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f);
|
||||
|
||||
// apply a 10 second first order low pass filter to baro offset
|
||||
float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt + _state.pos(
|
||||
2) - _baro_hgt_offset);
|
||||
const float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt + _state.pos(2) -
|
||||
_baro_hgt_offset);
|
||||
_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
|
||||
}
|
||||
}
|
||||
|
||||
Vector3f Ekf::getVisionVelocityInEkfFrame()
|
||||
Vector3f Ekf::getVisionVelocityInEkfFrame() const
|
||||
{
|
||||
Vector3f vel;
|
||||
// correct velocity for offset relative to IMU
|
||||
@@ -1433,7 +1432,7 @@ Vector3f Ekf::getVisionVelocityInEkfFrame()
|
||||
return vel;
|
||||
}
|
||||
|
||||
Vector3f Ekf::getVisionVelocityVarianceInEkfFrame()
|
||||
Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() const
|
||||
{
|
||||
Matrix3f ev_vel_cov = _ev_sample_delayed.velCov;
|
||||
|
||||
|
||||
@@ -186,7 +186,7 @@ void EstimatorInterface::setGpsData(const gps_message &gps)
|
||||
}
|
||||
|
||||
// limit data rate to prevent data being lost
|
||||
bool need_gps = (_params.fusion_mode & MASK_USE_GPS) || (_params.vdist_sensor_type == VDIST_SENSOR_GPS);
|
||||
const bool need_gps = (_params.fusion_mode & MASK_USE_GPS) || (_params.vdist_sensor_type == VDIST_SENSOR_GPS);
|
||||
|
||||
// TODO: remove checks that are not timing related
|
||||
if (((gps.time_usec - _time_last_gps) > _min_obs_interval_us) && need_gps && gps.fix_type > 2) {
|
||||
@@ -357,7 +357,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample& flow)
|
||||
// of min arrival interval because too much data is being lost
|
||||
float delta_time = flow.dt; // in seconds
|
||||
const float delta_time_min = 0.5e-6f * (float)_min_obs_interval_us;
|
||||
bool delta_time_good = delta_time >= delta_time_min;
|
||||
const bool delta_time_good = delta_time >= delta_time_min;
|
||||
|
||||
bool flow_magnitude_good = true;
|
||||
|
||||
@@ -510,14 +510,15 @@ void EstimatorInterface::setDragData()
|
||||
bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
||||
{
|
||||
// find the maximum time delay the buffers are required to handle
|
||||
uint16_t max_time_delay_ms = math::max(_params.mag_delay_ms,
|
||||
math::max(_params.range_delay_ms,
|
||||
math::max(_params.gps_delay_ms,
|
||||
const uint16_t max_time_delay_ms = math::max(_params.mag_delay_ms,
|
||||
math::max(_params.range_delay_ms,
|
||||
math::max(_params.gps_delay_ms,
|
||||
math::max(_params.flow_delay_ms,
|
||||
math::max(_params.ev_delay_ms,
|
||||
math::max(_params.auxvel_delay_ms,
|
||||
math::max(_params.min_delay_ms,
|
||||
math::max(_params.airspeed_delay_ms, _params.baro_delay_ms))))))));
|
||||
math::max(_params.ev_delay_ms,
|
||||
math::max(_params.auxvel_delay_ms,
|
||||
math::max(_params.min_delay_ms,
|
||||
math::max(_params.airspeed_delay_ms,
|
||||
_params.baro_delay_ms))))))));
|
||||
|
||||
// calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter
|
||||
_imu_buffer_length = (max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1;
|
||||
|
||||
@@ -202,7 +202,7 @@ void Ekf::fuseGpsYaw()
|
||||
|
||||
} else {
|
||||
// constrain the innovation to the maximum set by the gate
|
||||
float gate_limit = sqrtf((sq(innov_gate) * _heading_innov_var));
|
||||
const float gate_limit = sqrtf((sq(innov_gate) * _heading_innov_var));
|
||||
_heading_innov = math::constrain(_heading_innov, -gate_limit, gate_limit);
|
||||
}
|
||||
|
||||
|
||||
+4
-4
@@ -414,8 +414,8 @@ void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
|
||||
const float q2 = _state.quat_nominal(2);
|
||||
const float q3 = _state.quat_nominal(3);
|
||||
|
||||
float R_YAW = fmaxf(yaw_variance, 1.0e-4f);
|
||||
float measurement = wrap_pi(yaw);
|
||||
const float R_YAW = fmaxf(yaw_variance, 1.0e-4f);
|
||||
const float measurement = wrap_pi(yaw);
|
||||
float H_YAW[4];
|
||||
|
||||
// calculate observation jacobian when we are observing the first rotation in a 321 sequence
|
||||
@@ -509,8 +509,8 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
|
||||
const float q2 = _state.quat_nominal(2);
|
||||
const float q3 = _state.quat_nominal(3);
|
||||
|
||||
float R_YAW = fmaxf(yaw_variance, 1.0e-4f);
|
||||
float measurement = wrap_pi(yaw);
|
||||
const float R_YAW = fmaxf(yaw_variance, 1.0e-4f);
|
||||
const float measurement = wrap_pi(yaw);
|
||||
float H_YAW[4];
|
||||
|
||||
// calculate observation jacobian when we are observing a rotation in a 312 sequence
|
||||
|
||||
@@ -46,7 +46,7 @@
|
||||
|
||||
void Ekf::fuseSideslip()
|
||||
{
|
||||
float R_BETA = _params.beta_noise;
|
||||
const float R_BETA = _params.beta_noise;
|
||||
|
||||
// get latest estimated orientation
|
||||
const float q0 = _state.quat_nominal(0);
|
||||
|
||||
@@ -165,7 +165,7 @@ void Ekf::fuseHagl()
|
||||
|
||||
if (_hagl_test_ratio <= 1.0f) {
|
||||
// calculate the Kalman gain
|
||||
float gain = _terrain_var / _hagl_innov_var;
|
||||
const float gain = _terrain_var / _hagl_innov_var;
|
||||
// correct the state
|
||||
_terrain_vpos -= gain * _hagl_innov;
|
||||
// correct the variance
|
||||
|
||||
+12
-12
@@ -26,24 +26,24 @@ matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312)
|
||||
|
||||
float kahanSummation(float sum_previous, float input, float &accumulator)
|
||||
{
|
||||
float y = input - accumulator;
|
||||
float t = sum_previous + y;
|
||||
const float y = input - accumulator;
|
||||
const float t = sum_previous + y;
|
||||
accumulator = (t - sum_previous) - y;
|
||||
return t;
|
||||
}
|
||||
|
||||
matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat)
|
||||
{
|
||||
float q00 = quat(0) * quat(0);
|
||||
float q11 = quat(1) * quat(1);
|
||||
float q22 = quat(2) * quat(2);
|
||||
float q33 = quat(3) * quat(3);
|
||||
float q01 = quat(0) * quat(1);
|
||||
float q02 = quat(0) * quat(2);
|
||||
float q03 = quat(0) * quat(3);
|
||||
float q12 = quat(1) * quat(2);
|
||||
float q13 = quat(1) * quat(3);
|
||||
float q23 = quat(2) * quat(3);
|
||||
const float q00 = quat(0) * quat(0);
|
||||
const float q11 = quat(1) * quat(1);
|
||||
const float q22 = quat(2) * quat(2);
|
||||
const float q33 = quat(3) * quat(3);
|
||||
const float q01 = quat(0) * quat(1);
|
||||
const float q02 = quat(0) * quat(2);
|
||||
const float q03 = quat(0) * quat(3);
|
||||
const float q12 = quat(1) * quat(2);
|
||||
const float q13 = quat(1) * quat(3);
|
||||
const float q23 = quat(2) * quat(3);
|
||||
|
||||
matrix::Dcmf dcm;
|
||||
dcm(0, 0) = q00 + q11 - q22 - q33;
|
||||
|
||||
Reference in New Issue
Block a user