dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Remove un-used code

This commit is contained in:
Paul Riseborough 2021-03-09 20:32:35 +11:00 committed by Paul Riseborough
parent 4bad2a272c
commit b3c1bd6d40
2 changed files with 0 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
EKF/EKFGSF_yaw.cpp
View File

@ -97,32 +97,6 @@ void EKFGSF_yaw::update(const imuSample& imu_sample,
// all weights have collapsed due to excessive innovation variances so reset filters
initialiseEKFGSF();
}
// Enforce a minimum weighting value. This was added during initial development but has not been needed
// subsequently, so this block of code and the corresponding _weight_min can be removed if we get
// through testing without any weighting function issues.
if (_weight_min > FLT_EPSILON) {
float correction_sum = 0.0f; // amount the sum of weights has been increased by application of the limit
bool change_mask[N_MODELS_EKFGSF] = {}; // true when the weighting for that model has been increased
float unmodified_weights_sum = 0.0f; // sum of unmodified weights
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
if (_model_weights(model_index) < _weight_min) {
correction_sum += _weight_min - _model_weights(model_index);
_model_weights(model_index) = _weight_min;
change_mask[model_index] = true;
} else {
unmodified_weights_sum += _model_weights(model_index);
}
}
// rescale the unmodified weights to make the total sum unity
const float scale_factor = (unmodified_weights_sum - correction_sum - _weight_min) / (unmodified_weights_sum - _weight_min);
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
if (!change_mask[model_index]) {
_model_weights(model_index) = _weight_min + scale_factor * (_model_weights(model_index) - _weight_min);
}
}
}
}
}
} else if (_ekf_gsf_vel_fuse_started && !_run_ekf_gsf) {

1
EKF/EKFGSF_yaw.h
View File

@ -58,7 +58,6 @@ private:
const float _accel_noise{2.0f}; // horizontal accel noise used for covariance prediction (m/sec**2)
const float _tilt_gain{0.2f}; // gain from tilt error to gyro correction for complementary filter (1/sec)
const float _gyro_bias_gain{0.04f}; // gain applied to integral of gyro correction for complementary filter (1/sec)
const float _weight_min{0.0f}; // minimum value of an individual model weighting
// Declarations used by the bank of N_MODELS_EKFGSF AHRS complementary filters