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

EKF: Split angular alignment into tilt and yaw and use yaw and magnetic field alignment function

Browse Source
This commit is contained in:
Paul Riseborough
2016-02-14 22:01:53 +01:00
committed by Roman
parent 2bbe7f9a1c
commit aa58b3e98c
2 changed files with 17 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+8 -18
View File
@@ -246,27 +246,17 @@ bool Ekf::initialiseFilter(void)
return false;
}
// calculate initial tilt alignment
matrix::Euler<float> euler_init(roll, pitch, 0.0f);
_state.quat_nominal = Quaternion(euler_init);
_output_new.quat_nominal = _state.quat_nominal;
_control_status.flags.tilt_align = true;
// calculate the averaged magnetometer reading
Vector3f mag_init = _mag_sum * (1.0f / (float(_mag_counter)));
// rotate magnetic field into earth frame assuming zero yaw and estimate yaw angle assuming zero declination
// TODO use declination if available
matrix::Euler<float> euler_init(roll, pitch, 0.0f);
matrix::Dcm<float> R_to_earth_zeroyaw(euler_init);
Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init;
float declination = 0.0f;
euler_init(2) = declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0));
// calculate initial quaternion states
_state.quat_nominal = Quaternion(euler_init);
_output_new.quat_nominal = _state.quat_nominal;
// TODO replace this with a conditional test based on fitered angle error states.
_control_status.flags.angle_align = true;
// calculate initial earth magnetic field states
matrix::Dcm<float> R_to_earth(euler_init);
_state.mag_I = R_to_earth * mag_init;
// calculate the initial magnetic field and yaw alignment
_control_status.flags.yaw_align = resetMagHeading(mag_init);
// calculate the averaged barometer reading
_baro_at_alignment = _baro_sum / (float)_baro_counter;