fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 06:18:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: avoid constraining parameters every iteration

Browse Source
This commit is contained in:
Daniel Agar
2024-07-25 09:51:35 +02:00
committed by Mathieu Bresciani
parent ebcfb5348c
commit fd72578e98
2 changed files with 24 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/covariance.cpp
+7 -8
View File
@@ -115,11 +115,11 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
const float dt = 0.5f * (imu_delayed.delta_vel_dt + imu_delayed.delta_ang_dt);
// gyro noise variance
float gyro_noise = math::constrain(_params.gyro_noise, 0.f, 1.f);
float gyro_noise = _params.gyro_noise;
const float gyro_var = sq(gyro_noise);
// accel noise variance
float accel_noise = math::constrain(_params.accel_noise, 0.f, 1.f);
float accel_noise = _params.accel_noise;
Vector3f accel_var;
for (unsigned i = 0; i < 3; i++) {
@@ -143,7 +143,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
// gyro bias: add process noise
{
const float gyro_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.f, 1.f);
const float gyro_bias_sig = dt * _params.gyro_bias_p_noise;
const float gyro_bias_process_noise = sq(gyro_bias_sig);
for (unsigned index = 0; index < State::gyro_bias.dof; index++) {
@@ -157,7 +157,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
// accel bias: add process noise
{
const float accel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.f, 1.f);
const float accel_bias_sig = dt * _params.accel_bias_p_noise;
const float accel_bias_process_noise = sq(accel_bias_sig);
for (unsigned index = 0; index < State::accel_bias.dof; index++) {
@@ -172,7 +172,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
#if defined(CONFIG_EKF2_MAGNETOMETER)
// mag_I: add process noise
float mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.f, 1.f);
float mag_I_sig = dt * _params.mage_p_noise;
float mag_I_process_noise = sq(mag_I_sig);
for (unsigned index = 0; index < State::mag_I.dof; index++) {
@@ -184,7 +184,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
}
// mag_B: add process noise
float mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.f, 1.f);
float mag_B_sig = dt * _params.magb_p_noise;
float mag_B_process_noise = sq(mag_B_sig);
for (unsigned index = 0; index < State::mag_B.dof; index++) {
@@ -201,8 +201,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
#if defined(CONFIG_EKF2_WIND)
// wind vel: add process noise
float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f)
* (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
float wind_vel_nsd_scaled = _params.wind_vel_nsd * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
for (unsigned index = 0; index < State::wind_vel.dof; index++) {
src/modules/ekf2/EKF/ekf.cpp
+17
View File
@@ -132,6 +132,8 @@ void Ekf::reset()
}
_zero_velocity_update.reset();
updateParameters();
}
bool Ekf::update()
@@ -339,6 +341,21 @@ bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl
void Ekf::updateParameters()
{
_params.gyro_noise = math::constrain(_params.gyro_noise, 0.f, 1.f);
_params.accel_noise = math::constrain(_params.accel_noise, 0.f, 1.f);
_params.gyro_bias_p_noise = math::constrain(_params.gyro_bias_p_noise, 0.f, 1.f);
_params.accel_bias_p_noise = math::constrain(_params.accel_bias_p_noise, 0.f, 1.f);
#if defined(CONFIG_EKF2_MAGNETOMETER)
_params.mage_p_noise = math::constrain(_params.mage_p_noise, 0.f, 1.f);
_params.magb_p_noise = math::constrain(_params.magb_p_noise, 0.f, 1.f);
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_WIND)
_params.wind_vel_nsd = math::constrain(_params.wind_vel_nsd, 0.f, 1.f);
#endif // CONFIG_EKF2_WIND
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
_aux_global_position.updateParameters();
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION