ekf2: update earth rate also without GNSS

This commit is contained in:
bresch
2024-11-22 15:13:47 +01:00
committed by Mathieu Bresciani
parent 6becccb217
commit 842212df6c
4 changed files with 9 additions and 19 deletions
+5
View File
@@ -230,6 +230,11 @@ bool Ekf::initialiseTilt()
void Ekf::predictState(const imuSample &imu_delayed)
{
if (std::fabs(_gpos.latitude_rad() - _earth_rate_lat_ref_rad) > math::radians(1.0)) {
_earth_rate_lat_ref_rad = _gpos.latitude_rad();
_earth_rate_NED = calcEarthRateNED((float)_earth_rate_lat_ref_rad);
}
// apply imu bias corrections
const Vector3f delta_ang_bias_scaled = getGyroBias() * imu_delayed.delta_ang_dt;
Vector3f corrected_delta_ang = imu_delayed.delta_ang - delta_ang_bias_scaled;