From aa58b3e98c5de8db71bc62b5ef88a0214c944e0b Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 12 Feb 2016 14:23:44 +1100 Subject: [PATCH] EKF: Split angular alignment into tilt and yaw and use yaw and magnetic field alignment function --- EKF/common.h | 17 +++++++++-------- EKF/ekf.cpp | 26 ++++++++------------------ 2 files changed, 17 insertions(+), 26 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 39a865ff72..2e9b9f77ef 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -211,14 +211,15 @@ union gps_check_fail_status_u { // bitmask containing filter control status union filter_control_status_u { struct { - uint8_t angle_align : 1; // 0 - true if the filter angular alignment is complete - uint8_t gps : 1; // 1 - true if GPS measurements are being fused - uint8_t opt_flow : 1; // 2 - true if optical flow measurements are being fused - uint8_t mag_hdg : 1; // 3 - true if a simple magnetic heading is being fused - uint8_t mag_3D : 1; // 4 - true if 3-axis magnetometer measurement are being fused - uint8_t mag_dec : 1; // 5 - true if synthetic magnetic declination measurements are being fused - uint8_t in_air : 1; // 6 - true when the vehicle is airborne - uint8_t armed : 1; // 7 - true when the vehicle motors are armed + uint8_t tilt_align : 1; // 0 - true if the filter tilt alignment is complete + uint8_t yaw_align : 1; // 1 - true if the filter yaw alignment is complete + uint8_t gps : 1; // 2 - true if GPS measurements are being fused + uint8_t opt_flow : 1; // 3 - true if optical flow measurements are being fused + uint8_t mag_hdg : 1; // 4 - true if a simple magnetic heading is being fused + uint8_t mag_3D : 1; // 5 - true if 3-axis magnetometer measurement are being fused + uint8_t mag_dec : 1; // 6 - true if synthetic magnetic declination measurements are being fused + uint8_t in_air : 1; // 7 - true when the vehicle is airborne + uint8_t armed : 1; // 8 - true when the vehicle motors are armed } flags; uint16_t value; }; diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 599b4c6650..0e8a0267c1 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -246,27 +246,17 @@ bool Ekf::initialiseFilter(void) return false; } + // calculate initial tilt alignment + matrix::Euler 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 euler_init(roll, pitch, 0.0f); - matrix::Dcm 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 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;