fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 13:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

CleanUp of the EKFGSF (#769)

Browse Source 
* Add const modifiers

* Pass imu data as sampe

* Remove emergency reset request counter

* Improve matrix library usage

* Indentation fix

* Do not store innovation matrix for each model

* Make weights a separate vector

* EKF: Add missing alignment transfer to AHRS solutions

* EKF: Replace #define constants with static constexpr

* EKF: Move declaration for weights into GSF section

* EKF: Fix documentation error

Co-authored-by: Paul Riseborough <p_riseborough@live.com.au>
This commit is contained in:
kritz
2020-03-06 19:37:35 +11:00
committed by GitHub
co-authored by Paul Riseborough
parent 4669aa6312
commit bcd3869ca2
5 changed files with 102 additions and 133 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+5 -8
View File
@@ -1785,12 +1785,9 @@ bool Ekf::resetYawToEKFGSF()
}
void Ekf::requestEmergencyNavReset(uint8_t counter)
void Ekf::requestEmergencyNavReset()
{
if (counter > _yaw_extreset_counter) {
_yaw_extreset_counter = counter;
_do_emergency_yaw_reset = true;
}
_do_emergency_yaw_reset = true;
}
bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
@@ -1830,11 +1827,11 @@ void Ekf::runYawEKFGSF()
} else {
TAS = _airspeed_sample_delayed.true_airspeed;
}
yawEstimator.update(_imu_sample_delayed.delta_ang, _imu_sample_delayed.delta_vel, _imu_sample_delayed.delta_ang_dt, _imu_sample_delayed.delta_vel_dt, _control_status.flags.in_air, TAS);
yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS);
// basic sanity check on GPS velocity data
if (_gps_data_ready && _gps_sample_delayed.vacc > FLT_EPSILON && isfinite(_gps_sample_delayed.vel(0)) && isfinite(_gps_sample_delayed.vel(1))) {
Vector2f vel_NE = Vector2f(_gps_sample_delayed.vel(0),_gps_sample_delayed.vel(1));
yawEstimator.setVelocity(vel_NE, _gps_sample_delayed.vacc);
yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.vacc);
}
}