mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-23 03:36:37 +08:00
ekf2: update earth rate also without GNSS
This commit is contained in:
committed by
Mathieu Bresciani
parent
6becccb217
commit
842212df6c
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user