fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #4 from PX4/paul-wip

Browse Source 
EKF: Fix 'Dizzy on Startup' behaviour
This commit is contained in:
Lorenz Meier 2016-01-03 02:00:23 +01:00
commit b457709bd9
1 changed files with 18 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
EKF/ekf.cpp
View File

@ -133,7 +133,7 @@ bool Ekf::initialiseFilter(void)
_state.mag_B.setZero();
_state.wind_vel.setZero();
// get initial attitude estimate from accel vector, assuming vehicle is static
// get initial roll and pitch estimate from accel vector, assuming vehicle is static
Vector3f accel_init = _imu_down_sampled.delta_vel / _imu_down_sampled.delta_vel_dt;
float pitch = 0.0f;
@ -146,22 +146,30 @@ bool Ekf::initialiseFilter(void)
roll = -asinf(accel_init(1) / cosf(pitch));
}
magSample mag_init = _mag_buffer.get_newest();
matrix::Euler<float> euler_init(roll, pitch, 0.0f);
// Get the latest magnetic field measurement.
// If we don't have a measurement then we cannot initialise the filter
magSample mag_init = _mag_buffer.get_newest();
if (mag_init.time_us == 0) {
return false;
}
float yaw_init = atan2f(mag_init.mag(1), mag_init.mag(0));
// rotate magnetic field into earth frame assuming zero yaw and estimate yaw angle assuming zero declination
// TODO use declination if available
matrix::Dcm<float> R_to_earth_zeroyaw(euler_init);
Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init.mag;
float declination = 0.0f;
euler_init(2) = declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0));
matrix::Euler<float> euler_init(roll, pitch, yaw_init);
_state.quat_nominal = Quaternion(euler_init);
// calculate initial quaternion states
_state.quat_nominal = Quaternion(euler_init);
// calculate initial earth magnetic field states
matrix::Dcm<float> R_to_earth(euler_init);
_state.mag_I = R_to_earth * mag_init.mag;
matrix::Dcm<float> R_to_earth(euler_init);
_state.mag_I = R_to_earth * mag_init.mag;
resetVelocity();
resetVelocity();
resetPosition();
initialiseCovariance();