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

EKF: Enable planes to recover from bad mag data at start of flight

Browse Source 
Adjusts yaw by the amount of the error between GPS and EKF course if innovations are large.
This commit is contained in:
Paul Riseborough
2016-11-02 17:48:52 +11:00
committed by Lorenz Meier
parent 48a42dfb5b
commit f064915889
3 changed files with 168 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+12 -5
View File
@@ -946,7 +946,7 @@ void Ekf::controlBetaFusion()
if (!_control_status.flags.wind) {
// activate the wind states
_control_status.flags.wind = true;
// reset the timout timers to prevent repeated resets
// reset the timeout timers to prevent repeated resets
_time_last_beta_fuse = _time_last_imu;
_time_last_arsp_fuse = _time_last_imu;
// reset the wind speed states and corresponding covariances
@@ -996,7 +996,7 @@ void Ekf::controlMagFusion()
_flt_mag_align_complete = false;
}
// checs for new magnetometer data tath has fallen beind the fusion time horizon
// check for new magnetometer data that has fallen behind the fusion time horizon
if (_mag_data_ready) {
// Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances
@@ -1049,9 +1049,16 @@ void Ekf::controlMagFusion()
if (use_3D_fusion) {
if (!_control_status.flags.mag_3D) {
if (!_flt_mag_align_complete) {
// if transitioning into 3-axis fusion mode for the first time, we need to initialise the yaw angle and field states
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
_flt_mag_align_complete = true;
// If we are flying a vehicle that flies forward, eg plane, then we can use the GPS course to check and correct the heading
// if we are doing wind estimation and using the zero sideslip assumotion, then it is reasonable to assume the plane is flying forward
bool zeroSideslipValid = _control_status.flags.fuse_beta;
if (zeroSideslipValid) {
_flt_mag_align_complete = _control_status.flags.yaw_align = realignYawGPS();
} else {
_flt_mag_align_complete = _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
} else {
// reset the mag field covariances
zeroRows(P, 16, 21);
EKF/ekf.h
+4
View File
@@ -438,6 +438,10 @@ private:
// return true if successful
bool resetMagHeading(Vector3f &mag_init);
// Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.
// It is used to align the yaw angle after launch or takeoff for fixed wing vehicle.
bool realignYawGPS();
// calculate the magnetic declination to be used by the alignment and fusion processing
void calcMagDeclination();
EKF/ekf_helper.cpp
+152
View File
@@ -39,6 +39,7 @@
*
*/
#include "../ecl.h"
#include "ekf.h"
#include "mathlib.h"
#include <cstdlib>
@@ -336,6 +337,138 @@ void Ekf::alignOutputFilter()
}
}
// Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.
// It is used to align the yaw angle after launch or takeoff for fixed wing vehicle.
bool Ekf::realignYawGPS()
{
// Need at least 5 m/s of GPS horizontal speed and ratio of velocity error to velocity < 0.15 for a reliable alignment
float gpsSpeed = sqrtf(sq(_gps_sample_delayed.vel(0)) + sq(_gps_sample_delayed.vel(1)));
if ((gpsSpeed > 5.0f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed))) {
// calculate course yaw angle
float velYaw = atan2f(_state.vel(1),_state.vel(0));
// calculate course yaw angle from GPS velocity
float gpsYaw = atan2f(_gps_sample_delayed.vel(1),_gps_sample_delayed.vel(0));
// Check the EKF and GPS course for consistency
float courseYawError = gpsYaw - velYaw;
courseYawError = wrap_pi(courseYawError);
// If the angles disagree and horizontal GPS velocity innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad
bool badVelInnov = ((_vel_pos_test_ratio[0] > 1.0f) || (_vel_pos_test_ratio[1] > 1.0f)) && _control_status.flags.gps;
bool badYawErr = fabsf(courseYawError) > 0.5f;
bool badMagYaw = (badYawErr && badVelInnov) || !_control_status.flags.yaw_align;
// correct yaw angle using GPS ground course if compass yaw bad
if (badMagYaw) {
// save a copy of the quaternion state for later use in calculating the amount of reset change
Quatf quat_before_reset = _state.quat_nominal;
// calculate the variance for the rotation estimate expressed as a rotation vector
// this will be used later to reset the quaternion state covariances
Vector3f angle_err_var_vec = calcRotVecVariances();
// update transformation matrix from body to world frame using the current state estimate
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
// Use the best conditioned of a 321 or 312 Euler sequence to avoid gimbal lock
if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
// get quaternion from existing filter states and calculate roll, pitch and yaw angles
Eulerf euler321(_state.quat_nominal);
// calculate new filter quaternion states using previous Roll/Pitch and yaw angle corrected
// for ground course discrepancy
euler321(2) += courseYawError;
_state.quat_nominal = Quatf(euler321);
// update transformation matrix from body to world frame using the current state estimate
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
} else {
// Calculate the 312 sequence euler angles that rotate from earth to body frame
// See http://www.atacolorado.com/eulersequences.doc
Vector3f euler312;
euler312(0) = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)); // first rotation (yaw)
euler312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
euler312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch)
// correct yaw angle for ground course discrepancy
euler312(0) += courseYawError;
// Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence
float c2 = cosf(euler312(2));
float s2 = sinf(euler312(2));
float s1 = sinf(euler312(1));
float c1 = cosf(euler312(1));
float s0 = sinf(euler312(0));
float c0 = cosf(euler312(0));
_R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
_R_to_earth(1, 1) = c0 * c1;
_R_to_earth(2, 2) = c2 * c1;
_R_to_earth(0, 1) = -c1 * s0;
_R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0;
_R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0;
_R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2;
_R_to_earth(2, 0) = -s2 * c1;
_R_to_earth(2, 1) = s1;
_state.quat_nominal = Quatf(_R_to_earth);
}
// reset the velocity and posiiton states as they will be inaccurate due to bad yaw
resetVelocity();
resetPosition();
// Use the last magnetometer measurements to reset the field states
_state.mag_B.zero();
_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
// use the combined EKF and GPS speed variance to calculate a rough estimate of the yaw error after alignment
float SpdErrorVariance = sq(_gps_sample_delayed.sacc) + P[4][4] + P[5][5];
float sineYawError = math::constrain(sqrtf(SpdErrorVariance) / gpsSpeed, 0.0f, 1.0f);
angle_err_var_vec(2) = sq(asinf(sineYawError));
// reset the quaternion covariances using the rotation vector variances
initialiseQuatCovariances(angle_err_var_vec);
// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
zeroRows(P, 16, 21);
zeroCols(P, 16, 21);
for (uint8_t index = 16; index <= 21; index ++) {
P[index][index] = sq(_params.mag_noise);
}
// calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
// add the reset amount to the output observer buffered data
outputSample output_states;
unsigned output_length = _output_buffer.get_length();
for (unsigned i=0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal *= _state_reset_status.quat_change;
_output_buffer.push_to_index(i,output_states);
}
// apply the change in attitude quaternion to our newest quaternion estimate
// which was already taken out from the output buffer
_output_new.quat_nominal *= _state_reset_status.quat_change;
// capture the reset event
_state_reset_status.quat_counter++;
}
return true;
} else {
return false;
}
}
// Reset heading and magnetic field states
bool Ekf::resetMagHeading(Vector3f &mag_init)
{
@@ -383,6 +516,25 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
// we don't change the output attitude to avoid jumps
_state.quat_nominal = Quatf(euler321);
// calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
// add the reset amount to the output observer buffered data
outputSample output_states;
unsigned output_length = _output_buffer.get_length();
for (unsigned i=0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal *= _state_reset_status.quat_change;
_output_buffer.push_to_index(i,output_states);
}
// apply the change in attitude quaternion to our newest quaternion estimate
// which was already taken out from the output buffer
_output_new.quat_nominal *= _state_reset_status.quat_change;
// capture the reset event
_state_reset_status.quat_counter++;
} else {
// use a 312 sequence