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

EKF: Add Emergency yaw recovery using EKF-GSF estimator (#766)

Browse Source 
* EKF: Use common rate vector calculation for offset corrections

* EKF: Remove duplicate matrix entry calculations

* EKF: Create a EKF-GSF yaw estimator class

* EKF: add emergency yaw reset functionality

* EKF: remove un-used function

* EKF: Ensure required constants are defined for all builds

* EKF: Fix CI build error

* Revert "EKF: remove un-used function"

This reverts commit 93005309c7f3794414ad99c86218b3062e00bbd3.

* EKF: Replace in-lined Tait-Bryan 312 conversions with function call

Also remove unnecessary operations

* EKF: Remove unnecessary update of external vision rotation matrix

* EKF: Use const

* EKF: use const

* EKF: don't use class variable as a temporary variable

* EKF: update comments

* EKF: Improve efficiency of yaw reset

Use conversion from rotation matrix to Euler angles instead of quaternion to euler angles.

* EKF: use const

* EKF: remove un-used struct element

* EKF: more descriptive function name

* EKF: use existing matrix row operator

* EKF: remove unnecessary rotation matrix update

* EKF: Use square matrix type

* EKF: Improve protection for bad innovation covariance

* EKF: Use matrix library operations

* EKF: Replace memcpy with better alternative

memcpy bypasses compiler sanity checks and is unnecessary in this instance.

* EKF: Split EKF-GSF yaw reset function

Adds a common function to support yaw reset that can be used elsewhere.

* EKF: Use common function for quaternion state and covariance yaw reset

* EKF: Replace inlined matrix operation

* EKF: Use const

* EKF: Change accessor function name

* EKF: Use const

* EKF: Don't create unnecessary duplicate variable locations

* EKF: Remove duplicate covariance innovation inverse

* EKF: Don't create unnecessary duplicate variable locations

* EKF: Rely on geo library to provide gravity

* EKF: Improve protection from bad updates

* EKF: Reduce effect of vibration on yaw estimator AHRS

* EKF: Improve yaw estimator AHRS accuracy during manoeuvre transients
This commit is contained in:
Paul Riseborough 2020-03-05 21:50:52 +11:00 committed by GitHub
parent 8ce285cdfa
commit 4669aa6312
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
10 changed files with 1129 additions and 231 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
EKF/CMakeLists.txt
View File

@ -49,6 +49,7 @@ add_library(ecl_EKF
gps_yaw_fusion.cpp
range_finder_checks.cpp
imu_down_sampler.cpp
EKFGSF_yaw.cpp
)
add_dependencies(ecl_EKF prebuild_targets)

656
EKF/EKFGSF_yaw.cpp Normal file
View File

@ -0,0 +1,656 @@
#include "EKFGSF_yaw.h"
#include <cstdlib>
EKFGSF_yaw::EKFGSF_yaw()
{
// this flag must be false when we start
_ahrs_ekf_gsf_tilt_aligned = false;
// these objects are initialised in initialise() before being used internally, but can be reported for logging before then
memset(&_ahrs_ekf_gsf, 0, sizeof(_ahrs_ekf_gsf));
memset(&_ekf_gsf, 0, sizeof(_ekf_gsf));
_gsf_yaw = 0.0f;
_ahrs_accel.zero();
}
void EKFGSF_yaw::update(const Vector3f del_ang, // IMU delta angle rotation vector meassured in body frame (rad)
const Vector3f del_vel, // IMU delta velocity vector meassured in body frame (m/s)
const float del_ang_dt, // time interval that del_ang was integrated over (sec)
const float del_vel_dt, // time interval that del_vel was integrated over (sec)
bool run_EKF, // set to true when flying or movement is suitable for yaw estimation
float airspeed) // true airspeed used for centripetal accel compensation - set to 0 when not required.
{
// copy to class variables
_delta_ang = del_ang;
_delta_vel = del_vel;
_delta_ang_dt = del_ang_dt;
_delta_vel_dt = del_vel_dt;
_run_ekf_gsf = run_EKF;
_true_airspeed = airspeed;
// to reduce effect of vibration, filter using an LPF whose time constant is 1/10 of the AHRS tilt correction time constant
const float filter_coef = fminf(10.0f * _delta_vel_dt * _tilt_gain, 1.0f);
const Vector3f accel = _delta_vel / fmaxf(_delta_vel_dt, 0.001f);
_ahrs_accel = _ahrs_accel * (1.0f - filter_coef) + accel * filter_coef;
// Initialise states first time
if (!_ahrs_ekf_gsf_tilt_aligned) {
// check for excessive acceleration to reduce likelihood of large inital roll/pitch errors
// due to vehicle movement
const float accel_norm_sq = accel.norm_squared();
const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f;
const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f;
const bool ok_to_align = (accel_norm_sq > sq(lower_accel_limit)) && (accel_norm_sq < sq(upper_accel_limit));
if (ok_to_align) {
initialiseEKFGSF();
ahrsAlignTilt();
_ahrs_ekf_gsf_tilt_aligned = true;
}
return;
}
// AHRS prediction cycle for each model - this always runs
ahrsCalcAccelGain();
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
predictEKF(model_index);
}
// The 3-state EKF models only run when flying to avoid corrupted estimates due to operator handling and GPS interference
if (_run_ekf_gsf && _vel_data_updated) {
if (!_ekf_gsf_vel_fuse_started) {
initialiseEKFGSF();
ahrsAlignYaw();
_ekf_gsf_vel_fuse_started = true;
} else {
float total_w = 0.0f;
float newWeight[N_MODELS_EKFGSF];
bool bad_update = false;
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
// subsequent measurements are fused as direct state observations
if (!updateEKF(model_index)) {
bad_update = true;
}
}
if (!bad_update) {
// calculate weighting for each model assuming a normal distribution
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
newWeight[model_index] = fmaxf(gaussianDensity(model_index) * _ekf_gsf[model_index].W, 0.0f);
total_w += newWeight[model_index];
}
// normalise the weighting function
if (_ekf_gsf_vel_fuse_started && total_w > 1e-15f && !bad_update) {
float total_w_inv = 1.0f / total_w;
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
_ekf_gsf[model_index].W = newWeight[model_index] * total_w_inv;
}
}
// Enforce a minimum weighting value. This was added during initial development but has not been needed
// subsequently, so this block of code and the corresponding _weight_min can be removed if we get
// through testing without any weighting function issues.
if (_weight_min > FLT_EPSILON) {
float correction_sum = 0.0f; // amount the sum of weights has been increased by application of the limit
bool change_mask[N_MODELS_EKFGSF] = {}; // true when the weighting for that model has been increased
float unmodified_weights_sum = 0.0f; // sum of unmodified weights
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
if (_ekf_gsf[model_index].W < _weight_min) {
correction_sum += _weight_min - _ekf_gsf[model_index].W;
_ekf_gsf[model_index].W = _weight_min;
change_mask[model_index] = true;
} else {
unmodified_weights_sum += _ekf_gsf[model_index].W;
}
}
// rescale the unmodified weights to make the total sum unity
const float scale_factor = (unmodified_weights_sum - correction_sum - _weight_min) / (unmodified_weights_sum - _weight_min);
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
if (!change_mask[model_index]) {
_ekf_gsf[model_index].W = _weight_min + scale_factor * (_ekf_gsf[model_index].W - _weight_min);
}
}
}
}
}
} else if (_ekf_gsf_vel_fuse_started && !_run_ekf_gsf) {
// wait to fly again
_ekf_gsf_vel_fuse_started = false;
}
// Calculate a composite yaw vector as a weighted average of the states for each model.
// To avoid issues with angle wrapping, the yaw state is converted to a vector with legnth
// equal to the weighting value before it is summed.
Vector2f yaw_vector = {};
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
yaw_vector(0) += _ekf_gsf[model_index].W * cosf(_ekf_gsf[model_index].X(2));
yaw_vector(1) += _ekf_gsf[model_index].W * sinf(_ekf_gsf[model_index].X(2));
}
_gsf_yaw = atan2f(yaw_vector(1),yaw_vector(0));
// calculate a composite variance for the yaw state from a weighted average of the variance for each model
// models with larger innovations are weighted less
_gsf_yaw_variance = 0.0f;
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
float yaw_delta = wrap_pi(_ekf_gsf[model_index].X(2) - _gsf_yaw);
_gsf_yaw_variance += _ekf_gsf[model_index].W * (_ekf_gsf[model_index].P(2,2) + yaw_delta * yaw_delta);
}
// prevent the same velocity data being used more than once
_vel_data_updated = false;
}
void EKFGSF_yaw::ahrsPredict(const uint8_t model_index)
{
// generate attitude solution using simple complementary filter for the selected model
Vector3f ang_rate = _delta_ang / fmaxf(_delta_ang_dt, 0.001f) - _ahrs_ekf_gsf[model_index].gyro_bias;
// Accelerometer correction
// Project 'k' unit vector of earth frame to body frame
// Vector3f k = quaterion.conjugate_inversed(Vector3f(0.0f, 0.0f, 1.0f));
// Optimized version with dropped zeros
const Vector3f k(_ahrs_ekf_gsf[model_index].R(2,0), _ahrs_ekf_gsf[model_index].R(2,1), _ahrs_ekf_gsf[model_index].R(2,2));
// Perform angular rate correction using accel data and reduce correction as accel magnitude moves away from 1 g (reduces drift when vehicle picked up and moved).
// During fixed wing flight, compensate for centripetal acceleration assuming coordinated turns and X axis forward
Vector3f tilt_correction = {};
if (_ahrs_accel_fusion_gain > 0.0f) {
Vector3f accel = _ahrs_accel;
if (_true_airspeed > FLT_EPSILON) {
// turn rate is component of gyro rate about vertical (down) axis
const float turn_rate = _ahrs_ekf_gsf[model_index].R(2,0) * ang_rate(0)
+ _ahrs_ekf_gsf[model_index].R(2,1) * ang_rate(1)
+ _ahrs_ekf_gsf[model_index].R(2,2) * ang_rate(2);
// use measured airspeed to calculate centripetal acceleration if available
float centripetal_accel = _true_airspeed * turn_rate;
// project Y body axis onto horizontal and multiply by centripetal acceleration to give estimated
// centripetal acceleration vector in earth frame due to coordinated turn
Vector3f centripetal_accel_vec_ef = {_ahrs_ekf_gsf[model_index].R(0,1), _ahrs_ekf_gsf[model_index].R(1,1), 0.0f};
if (_ahrs_ekf_gsf[model_index].R(2,2) > 0.0f) {
// vehicle is upright
centripetal_accel_vec_ef *= centripetal_accel;
} else {
// vehicle is inverted
centripetal_accel_vec_ef *= - centripetal_accel;
}
// rotate into body frame
Vector3f centripetal_accel_vec_bf = _ahrs_ekf_gsf[model_index].R.transpose() * centripetal_accel_vec_ef;
// correct measured accel for centripetal acceleration
accel -= centripetal_accel_vec_bf;
}
tilt_correction = (k % accel) * _ahrs_accel_fusion_gain / _ahrs_accel_norm;
}
// Gyro bias estimation
const float gyro_bias_limit = 0.05f;
const float spinRate = ang_rate.length();
if (spinRate < 0.175f) {
_ahrs_ekf_gsf[model_index].gyro_bias -= tilt_correction * (_gyro_bias_gain * _delta_ang_dt);
for (int i = 0; i < 3; i++) {
_ahrs_ekf_gsf[model_index].gyro_bias(i) = math::constrain(_ahrs_ekf_gsf[model_index].gyro_bias(i), -gyro_bias_limit, gyro_bias_limit);
}
}
// delta angle from previous to current frame
Vector3f delta_angle_corrected = _delta_ang + (tilt_correction - _ahrs_ekf_gsf[model_index].gyro_bias) * _delta_ang_dt;
// Apply delta angle to rotation matrix
_ahrs_ekf_gsf[model_index].R = ahrsPredictRotMat(_ahrs_ekf_gsf[model_index].R, delta_angle_corrected);
}
void EKFGSF_yaw::ahrsAlignTilt()
{
// Rotation matrix is constructed directly from acceleration measurement and will be the same for
// all models so only need to calculate it once. Assumptions are:
// 1) Yaw angle is zero - yaw is aligned later for each model when velocity fusion commences.
// 2) The vehicle is not accelerating so all of the measured acceleration is due to gravity.
// Calculate earth frame Down axis unit vector rotated into body frame
Vector3f down_in_bf = -_delta_vel;
down_in_bf.normalize();
// Calculate earth frame North axis unit vector rotated into body frame, orthogonal to 'down_in_bf'
// * operator is overloaded to provide a dot product
const Vector3f i_vec_bf(1.0f,0.0f,0.0f);
Vector3f north_in_bf = i_vec_bf - down_in_bf * (i_vec_bf * down_in_bf);
north_in_bf.normalize();
// Calculate earth frame East axis unit vector rotated into body frame, orthogonal to 'down_in_bf' and 'north_in_bf'
// % operator is overloaded to provide a cross product
const Vector3f east_in_bf = down_in_bf % north_in_bf;
// Each column in a rotation matrix from earth frame to body frame represents the projection of the
// corresponding earth frame unit vector rotated into the body frame, eg 'north_in_bf' would be the first column.
// We need the rotation matrix from body frame to earth frame so the earth frame unit vectors rotated into body
// frame are copied into corresponding rows instead.
Dcmf R;
R.setRow(0, north_in_bf);
R.setRow(1, east_in_bf);
R.setRow(2, down_in_bf);
}
void EKFGSF_yaw::ahrsAlignYaw()
{
// Align yaw angle for each model
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
if (fabsf(_ahrs_ekf_gsf[model_index].R(2, 0)) < fabsf(_ahrs_ekf_gsf[model_index].R(2, 1))) {
// get the roll, pitch, yaw estimates from the rotation matrix using a 321 Tait-Bryan rotation sequence
Eulerf euler_init(_ahrs_ekf_gsf[model_index].R);
// set the yaw angle
euler_init(2) = wrap_pi(_ekf_gsf[model_index].X(2));
// update the rotation matrix
_ahrs_ekf_gsf[model_index].R = Dcmf(euler_init);
} else {
// Calculate the 312 Tait-Bryan rotation sequence that rotates from earth to body frame
Vector3f rot312;
rot312(0) = wrap_pi(_ekf_gsf[model_index].X(2)); // first rotation (yaw) taken from EKF model state
rot312(1) = asinf(_ahrs_ekf_gsf[model_index].R(2, 1)); // second rotation (roll)
rot312(2) = atan2f(-_ahrs_ekf_gsf[model_index].R(2, 0), _ahrs_ekf_gsf[model_index].R(2, 2)); // third rotation (pitch)
// Calculate the body to earth frame rotation matrix
_ahrs_ekf_gsf[model_index].R = taitBryan312ToRotMat(rot312);
}
_ahrs_ekf_gsf[model_index].aligned = true;
}
}
void EKFGSF_yaw::predictEKF(const uint8_t model_index)
{
// generate an attitude reference using IMU data
ahrsPredict(model_index);
// we don't start running the EKF part of the algorithm until there are regular velocity observations
if (!_ekf_gsf_vel_fuse_started) {
return;
}
// Calculate the yaw state using a projection onto the horizontal that avoids gimbal lock
if (fabsf(_ahrs_ekf_gsf[model_index].R(2, 0)) < fabsf(_ahrs_ekf_gsf[model_index].R(2, 1))) {
// use 321 Tait-Bryan rotation to define yaw state
_ekf_gsf[model_index].X(2) = atan2f(_ahrs_ekf_gsf[model_index].R(1, 0), _ahrs_ekf_gsf[model_index].R(0, 0));
} else {
// use 312 Tait-Bryan rotation to define yaw state
_ekf_gsf[model_index].X(2) = atan2f(-_ahrs_ekf_gsf[model_index].R(0, 1), _ahrs_ekf_gsf[model_index].R(1, 1)); // first rotation (yaw)
}
// calculate delta velocity in a horizontal front-right frame
const Vector3f del_vel_NED = _ahrs_ekf_gsf[model_index].R * _delta_vel;
const float dvx = del_vel_NED(0) * cosf(_ekf_gsf[model_index].X(2)) + del_vel_NED(1) * sinf(_ekf_gsf[model_index].X(2));
const float dvy = - del_vel_NED(0) * sinf(_ekf_gsf[model_index].X(2)) + del_vel_NED(1) * cosf(_ekf_gsf[model_index].X(2));
// sum delta velocities in earth frame:
_ekf_gsf[model_index].X(0) += del_vel_NED(0);
_ekf_gsf[model_index].X(1) += del_vel_NED(1);
// predict covariance - autocode from https://github.com/priseborough/3_state_filter/blob/flightLogReplay-wip/calcPupdate.txt
// Local short variable name copies required for readability
// Compiler might be smart enough to optimise these out
const float &P00 = _ekf_gsf[model_index].P(0,0);
const float &P01 = _ekf_gsf[model_index].P(0,1);
const float &P02 = _ekf_gsf[model_index].P(0,2);
const float &P10 = _ekf_gsf[model_index].P(1,0);
const float &P11 = _ekf_gsf[model_index].P(1,1);
const float &P12 = _ekf_gsf[model_index].P(1,2);
const float &P20 = _ekf_gsf[model_index].P(2,0);
const float &P21 = _ekf_gsf[model_index].P(2,1);
const float &P22 = _ekf_gsf[model_index].P(2,2);
// Use fixed values for delta velocity and delta angle process noise variances
const float dvxVar = sq(_accel_noise * _delta_vel_dt); // variance of forward delta velocity - (m/s)^2
const float dvyVar = dvxVar; // variance of right delta velocity - (m/s)^2
const float dazVar = sq(_gyro_noise * _delta_ang_dt); // variance of yaw delta angle - rad^2
const float t2 = sinf(_ekf_gsf[model_index].X(2));
const float t3 = cosf(_ekf_gsf[model_index].X(2));
const float t4 = dvy*t3;
const float t5 = dvx*t2;
const float t6 = t4+t5;
const float t8 = P22*t6;
const float t7 = P02-t8;
const float t9 = dvx*t3;
const float t11 = dvy*t2;
const float t10 = t9-t11;
const float t12 = dvxVar*t2*t3;
const float t13 = t2*t2;
const float t14 = t3*t3;
const float t15 = P22*t10;
const float t16 = P12+t15;
const float min_var = 1e-6f;
_ekf_gsf[model_index].P(0,0) = fmaxf(P00-P20*t6+dvxVar*t14+dvyVar*t13-t6*t7 , min_var);
_ekf_gsf[model_index].P(0,1) = P01+t12-P21*t6+t7*t10-dvyVar*t2*t3;
_ekf_gsf[model_index].P(0,2) = t7;
_ekf_gsf[model_index].P(1,0) = P10+t12+P20*t10-t6*t16-dvyVar*t2*t3;
_ekf_gsf[model_index].P(1,1) = fmaxf(P11+P21*t10+dvxVar*t13+dvyVar*t14+t10*t16 , min_var);
_ekf_gsf[model_index].P(1,2) = t16;
_ekf_gsf[model_index].P(2,0) = P20-t8;
_ekf_gsf[model_index].P(2,1) = P21+t15;
_ekf_gsf[model_index].P(2,2) = fmaxf(P22+dazVar , min_var);
// force symmetry
_ekf_gsf[model_index].P.makeBlockSymmetric<3>(0);
}
// Update EKF states and covariance for specified model index using velocity measurement
bool EKFGSF_yaw::updateEKF(const uint8_t model_index)
{
// set observation variance from accuracy estimate supplied by GPS and apply a sanity check minimum
const float velObsVar = sq(fmaxf(_vel_accuracy, 0.5f));
// calculate velocity observation innovations
_ekf_gsf[model_index].innov(0) = _ekf_gsf[model_index].X(0) - _vel_NE(0);
_ekf_gsf[model_index].innov(1) = _ekf_gsf[model_index].X(1) - _vel_NE(1);
// Use temporary variables for covariance elements to reduce verbosity of auto-code expressions
const float &P00 = _ekf_gsf[model_index].P(0,0);
const float &P01 = _ekf_gsf[model_index].P(0,1);
const float &P02 = _ekf_gsf[model_index].P(0,2);
const float &P10 = _ekf_gsf[model_index].P(1,0);
const float &P11 = _ekf_gsf[model_index].P(1,1);
const float &P12 = _ekf_gsf[model_index].P(1,2);
const float &P20 = _ekf_gsf[model_index].P(2,0);
const float &P21 = _ekf_gsf[model_index].P(2,1);
const float &P22 = _ekf_gsf[model_index].P(2,2);
// calculate innovation variance
_ekf_gsf[model_index].S(0,0) = P00 + velObsVar;
_ekf_gsf[model_index].S(1,1) = P11 + velObsVar;
_ekf_gsf[model_index].S(0,1) = P01;
_ekf_gsf[model_index].S(1,0) = P10;
// Update the inverse of the innovation covariance matrix S_inverse
updateInnovCovMatInv(model_index);
// Perform a chi-square innovation consistency test and calculate a compression scale factor that limits the magnitude of innovations to 5-sigma
float innov_comp_scale_factor = 1.0f;
// test ratio = transpose(innovation) * inverse(innovation variance) * innovation = [1x2] * [2,2] * [2,1] = [1,1]
const float test_ratio = _ekf_gsf[model_index].innov * (_ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov);
// If the test ratio is greater than 25 (5 Sigma) then reduce the length of the innovation vector to clip it at 5-Sigma
// This protects from large measurement spikes
if (test_ratio > 25.0f) {
innov_comp_scale_factor = sqrtf(25.0f / test_ratio);
}
// calculate Kalman gain K nd covariance matrix P
// autocode from https://github.com/priseborough/3_state_filter/blob/flightLogReplay-wip/calcK.txt
// and https://github.com/priseborough/3_state_filter/blob/flightLogReplay-wip/calcPmat.txt
const float t2 = P00*velObsVar;
const float t3 = P11*velObsVar;
const float t4 = velObsVar*velObsVar;
const float t5 = P00*P11;
const float t9 = P01*P10;
const float t6 = t2+t3+t4+t5-t9;
float t7;
if (fabsf(t6) > 1e-6f) {
t7 = 1.0f/t6;
} else {
// skip this fusion step
return false;
}
const float t8 = P11+velObsVar;
const float t10 = P00+velObsVar;
matrix::Matrix<float, 3, 2> K;
K(0,0) = -P01*P10*t7+P00*t7*t8;
K(0,1) = -P00*P01*t7+P01*t7*t10;
K(1,0) = -P10*P11*t7+P10*t7*t8;
K(1,1) = -P01*P10*t7+P11*t7*t10;
K(2,0) = -P10*P21*t7+P20*t7*t8;
K(2,1) = -P01*P20*t7+P21*t7*t10;
const float t11 = P00*P01*t7;
const float t15 = P01*t7*t10;
const float t12 = t11-t15;
const float t13 = P01*P10*t7;
const float t16 = P00*t7*t8;
const float t14 = t13-t16;
const float t17 = t8*t12;
const float t18 = P01*t14;
const float t19 = t17+t18;
const float t20 = t10*t14;
const float t21 = P10*t12;
const float t22 = t20+t21;
const float t27 = P11*t7*t10;
const float t23 = t13-t27;
const float t24 = P10*P11*t7;
const float t26 = P10*t7*t8;
const float t25 = t24-t26;
const float t28 = t8*t23;
const float t29 = P01*t25;
const float t30 = t28+t29;
const float t31 = t10*t25;
const float t32 = P10*t23;
const float t33 = t31+t32;
const float t34 = P01*P20*t7;
const float t38 = P21*t7*t10;
const float t35 = t34-t38;
const float t36 = P10*P21*t7;
const float t39 = P20*t7*t8;
const float t37 = t36-t39;
const float t40 = t8*t35;
const float t41 = P01*t37;
const float t42 = t40+t41;
const float t43 = t10*t37;
const float t44 = P10*t35;
const float t45 = t43+t44;
const float min_var = 1e-6f;
_ekf_gsf[model_index].P(0,0) = fmaxf(P00-t12*t19-t14*t22 , min_var);
_ekf_gsf[model_index].P(0,1) = P01-t19*t23-t22*t25;
_ekf_gsf[model_index].P(0,2) = P02-t19*t35-t22*t37;
_ekf_gsf[model_index].P(1,0) = P10-t12*t30-t14*t33;
_ekf_gsf[model_index].P(1,1) = fmaxf(P11-t23*t30-t25*t33 , min_var);
_ekf_gsf[model_index].P(1,2) = P12-t30*t35-t33*t37;
_ekf_gsf[model_index].P(2,0) = P20-t12*t42-t14*t45;
_ekf_gsf[model_index].P(2,1) = P21-t23*t42-t25*t45;
_ekf_gsf[model_index].P(2,2) = fmaxf(P22-t35*t42-t37*t45 , min_var);
// force symmetry
_ekf_gsf[model_index].P.makeBlockSymmetric<3>(0);
// Correct the state vector and capture the change in yaw angle
const float oldYaw = _ekf_gsf[model_index].X(2);
_ekf_gsf[model_index].X -= (K * _ekf_gsf[model_index].innov) * innov_comp_scale_factor;
float yawDelta = _ekf_gsf[model_index].X(2) - oldYaw;
// apply the change in yaw angle to the AHRS
// take advantage of sparseness in the yaw rotation matrix
const float cosYaw = cosf(yawDelta);
const float sinYaw = sinf(yawDelta);
float R_prev[2][3];
memcpy(&R_prev, &_ahrs_ekf_gsf[model_index].R, sizeof(R_prev));
_ahrs_ekf_gsf[model_index].R(0,0) = R_prev[0][0] * cosYaw - R_prev[1][0] * sinYaw;
_ahrs_ekf_gsf[model_index].R(0,1) = R_prev[0][1] * cosYaw - R_prev[1][1] * sinYaw;
_ahrs_ekf_gsf[model_index].R(0,2) = R_prev[0][2] * cosYaw - R_prev[1][2] * sinYaw;
_ahrs_ekf_gsf[model_index].R(1,0) = R_prev[0][0] * sinYaw + R_prev[1][0] * cosYaw;
_ahrs_ekf_gsf[model_index].R(1,1) = R_prev[0][1] * sinYaw + R_prev[1][1] * cosYaw;
_ahrs_ekf_gsf[model_index].R(1,2) = R_prev[0][2] * sinYaw + R_prev[1][2] * cosYaw;
return true;
}
void EKFGSF_yaw::initialiseEKFGSF()
{
_gsf_yaw = 0.0f;
_ekf_gsf_vel_fuse_started = false;
_gsf_yaw_variance = M_PI_2_F * M_PI_2_F;
memset(&_ekf_gsf, 0, sizeof(_ekf_gsf));
const float yaw_increment = 2.0f * M_PI_F / (float)N_MODELS_EKFGSF;
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
// evenly space initial yaw estimates in the region between +-Pi
_ekf_gsf[model_index].X(2) = -M_PI_F + (0.5f * yaw_increment) + ((float)model_index * yaw_increment);
// All filter models start with the same weight
_ekf_gsf[model_index].W = 1.0f / (float)N_MODELS_EKFGSF;
// take velocity states and corresponding variance from last meaurement
_ekf_gsf[model_index].X(0) = _vel_NE(0);
_ekf_gsf[model_index].X(1) = _vel_NE(1);
_ekf_gsf[model_index].P(0,0) = sq(_vel_accuracy);
_ekf_gsf[model_index].P(1,1) = _ekf_gsf[model_index].P(0,0);
// use half yaw interval for yaw uncertainty
_ekf_gsf[model_index].P(2,2) = sq(0.5f * yaw_increment);
}
}
float EKFGSF_yaw::gaussianDensity(const uint8_t model_index) const
{
// calculate inv(S) * innovation
const matrix::Vector2f tempVec = _ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov;
// calculate transpose(innovation) * inv(S) * innovation
// * operator is overloaded to provide a dot product
const float normDist = _ekf_gsf[model_index].innov * tempVec;
return M_TWOPI_INV * sqrtf(_ekf_gsf[model_index].S_det_inverse) * expf(-0.5f * normDist);
}
void EKFGSF_yaw::updateInnovCovMatInv(const uint8_t model_index)
{
// calculate determinant for innovation covariance matrix
const float t2 = _ekf_gsf[model_index].S(0,0) * _ekf_gsf[model_index].S(1,1);
const float t5 = _ekf_gsf[model_index].S(0,1) * _ekf_gsf[model_index].S(1,0);
const float t3 = t2 - t5;
// calculate determinant inverse and protect against badly conditioned matrix
_ekf_gsf[model_index].S_det_inverse = 1.0f / fmaxf(t3 , 1e-12f);
// calculate inv(S)
_ekf_gsf[model_index].S_inverse(0,0) = _ekf_gsf[model_index].S_det_inverse * _ekf_gsf[model_index].S(1,1);
_ekf_gsf[model_index].S_inverse(1,1) = _ekf_gsf[model_index].S_det_inverse * _ekf_gsf[model_index].S(0,0);
_ekf_gsf[model_index].S_inverse(0,1) = - _ekf_gsf[model_index].S_det_inverse * _ekf_gsf[model_index].S(0,1);
_ekf_gsf[model_index].S_inverse(1,0) = - _ekf_gsf[model_index].S_det_inverse * _ekf_gsf[model_index].S(1,0);
}
bool EKFGSF_yaw::getLogData(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
{
if (_ekf_gsf_vel_fuse_started) {
*yaw_composite = _gsf_yaw;
*yaw_variance = _gsf_yaw_variance;
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
yaw[model_index] = _ekf_gsf[model_index].X(2);
innov_VN[model_index] = _ekf_gsf[model_index].innov(0);
innov_VE[model_index] = _ekf_gsf[model_index].innov(1);
weight[model_index] = _ekf_gsf[model_index].W;
}
return true;
}
return false;
}
Dcmf EKFGSF_yaw::taitBryan312ToRotMat(const Vector3f &rot312)
{
// Calculate the frame2 to frame 1 rotation matrix from a 312 rotation sequence
const float c2 = cosf(rot312(2));
const float s2 = sinf(rot312(2));
const float s1 = sinf(rot312(1));
const float c1 = cosf(rot312(1));
const float s0 = sinf(rot312(0));
const float c0 = cosf(rot312(0));
Dcmf R;
R(0, 0) = c0 * c2 - s0 * s1 * s2;
R(1, 1) = c0 * c1;
R(2, 2) = c2 * c1;
R(0, 1) = -c1 * s0;
R(0, 2) = s2 * c0 + c2 * s1 * s0;
R(1, 0) = c2 * s0 + s2 * s1 * c0;
R(1, 2) = s0 * s2 - s1 * c0 * c2;
R(2, 0) = -s2 * c1;
R(2, 1) = s1;
return R;
}
void EKFGSF_yaw::ahrsCalcAccelGain()
{
// calculate common values used by the AHRS complementary filter models
_ahrs_accel_norm = _ahrs_accel.norm();
// Calculate the acceleration fusion gain using a continuous function that is unity at 1g and zero
// at the min and mas g value. Allow for more acceleration when flying as a fixed wing vehicle using centripetal
// acceleration correction as higher and more sustained g will be experienced.
// Use a quadratic finstead of linear unction to prevent vibration around 1g reducing the tilt correction effectiveness.
float accel_g = _ahrs_accel_norm / CONSTANTS_ONE_G;
if (accel_g > 1.0f) {
if (_true_airspeed > FLT_EPSILON && accel_g < 2.0f) {
_ahrs_accel_fusion_gain = _tilt_gain * sq(2.0f - accel_g);
} else if (accel_g < 1.5f) {
_ahrs_accel_fusion_gain = _tilt_gain * sq(3.0f - 2.0f * accel_g);
} else {
_ahrs_accel_fusion_gain = 0.0f;
}
} else if (accel_g > 0.5f) {
_ahrs_accel_fusion_gain = _tilt_gain * sq(2.0f * accel_g - 1.0f);
} else {
_ahrs_accel_fusion_gain = 0.0f;
}
}
Matrix3f EKFGSF_yaw::ahrsPredictRotMat(Matrix3f &R, Vector3f &g)
{
Matrix3f ret = R;
ret(0,0) += R(0,1) * g(2) - R(0,2) * g(1);
ret(0,1) += R(0,2) * g(0) - R(0,0) * g(2);
ret(0,2) += R(0,0) * g(1) - R(0,1) * g(0);
ret(1,0) += R(1,1) * g(2) - R(1,2) * g(1);
ret(1,1) += R(1,2) * g(0) - R(1,0) * g(2);
ret(1,2) += R(1,0) * g(1) - R(1,1) * g(0),
ret(2,0) += R(2,1) * g(2) - R(2,2) * g(1);
ret(2,1) += R(2,2) * g(0) - R(2,0) * g(2);
ret(2,2) += R(2,0) * g(1) - R(2,1) * g(0);
// Renormalise rows
float rowLengthSq;
for (uint8_t r = 0; r < 3; r++) {
rowLengthSq = ret.row(r).norm_squared();
if (rowLengthSq > FLT_EPSILON) {
// Use linear approximation for inverse sqrt taking advantage of the row length being close to 1.0
const float rowLengthInv = 1.5f - 0.5f * rowLengthSq;
ret(r,0) *= rowLengthInv;
ret(r,1) *= rowLengthInv;
ret(r,2) *= rowLengthInv;
}
}
return ret;
}
bool EKFGSF_yaw::getYawData(float *yaw, float *yaw_variance)
{
if(_ekf_gsf_vel_fuse_started) {
*yaw = _gsf_yaw;
*yaw_variance = _gsf_yaw_variance;
return true;
}
return false;
}
void EKFGSF_yaw::setVelocity(Vector2f velocity, float accuracy)
{
_vel_NE = velocity;
_vel_accuracy = accuracy;
_vel_data_updated = true;
}

153
EKF/EKFGSF_yaw.h Normal file
View File

@ -0,0 +1,153 @@
#pragma once
#include <geo/geo.h>
#include <matrix/math.hpp>
#include <mathlib/mathlib.h>
using matrix::AxisAnglef;
using matrix::Dcmf;
using matrix::Eulerf;
using matrix::Matrix3f;
using matrix::Quatf;
using matrix::Vector2f;
using matrix::Vector3f;
using matrix::wrap_pi;
#define N_MODELS_EKFGSF 5
#ifndef M_PI_F
#define M_PI_F 3.14159265f
#endif
#ifndef M_PI_2_F
#define M_PI_2_F 1.57079632f
#endif
#ifndef M_TWOPI_INV
#define M_TWOPI_INV 0.159154943f
#endif
class EKFGSF_yaw
{
public:
// Constructor
EKFGSF_yaw();
// Update Filter States - this should be called whenever new IMU data is available
void update(const Vector3f del_ang, // IMU delta angle rotation vector meassured in body frame (rad)
const Vector3f del_vel, // IMU delta velocity vector meassured in body frame (m/s)
const float del_ang_dt, // time interval that del_ang was integrated over (sec)
const float del_vel_dt, // time interval that del_vel was integrated over (sec)
bool run_EKF, // set to true when flying or movement is suitable for yaw estimation
float airspeed); // true airspeed used for centripetal accel compensation - set to 0 when not required.
void setVelocity(Vector2f velocity, // NE velocity measurement (m/s)
float accuracy); // 1-sigma accuracy of velocity measurement (m/s)
// get solution data for logging
bool getLogData(float *yaw_composite,
float *yaw_composite_variance,
float yaw[N_MODELS_EKFGSF],
float innov_VN[N_MODELS_EKFGSF],
float innov_VE[N_MODELS_EKFGSF],
float weight[N_MODELS_EKFGSF]);
// get yaw estimate and the corresponding variance
// return false if no yaw estimate available
bool getYawData(float *yaw, float *yaw_variance);
private:
// Parameters - these could be made tuneable
const float _gyro_noise{1.0e-1f}; // yaw rate noise used for covariance prediction (rad/sec)
const float _accel_noise{2.0f}; // horizontal accel noise used for covariance prediction (m/sec**2)
const float _tilt_gain{0.2f}; // gain from tilt error to gyro correction for complementary filter (1/sec)
const float _gyro_bias_gain{0.04f}; // gain applied to integral of gyro correction for complementary filter (1/sec)
const float _weight_min{0.0f}; // minimum value of an individual model weighting
// Declarations used by the bank of N_MODELS_EKFGSF AHRS complementary filters
Vector3f _delta_ang; // IMU delta angle (rad)
Vector3f _delta_vel; // IMU delta velocity (m/s)
float _delta_ang_dt; // _delta_ang integration time interval (sec)
float _delta_vel_dt; // _delta_vel integration time interval (sec)
float _true_airspeed; // true airspeed used for centripetal accel compensation (m/s)
struct _ahrs_ekf_gsf_struct{
Dcmf R; // matrix that rotates a vector from body to earth frame
Vector3f gyro_bias; // gyro bias learned and used by the quaternion calculation
bool aligned{false}; // true when AHRS has been aligned
float vel_NE[2] {}; // NE velocity vector from last GPS measurement (m/s)
bool fuse_gps = false; // true when GPS should be fused on that frame
float accel_dt = 0; // time step used when generating _simple_accel_FR data (sec)
};
_ahrs_ekf_gsf_struct _ahrs_ekf_gsf[N_MODELS_EKFGSF];
bool _ahrs_ekf_gsf_tilt_aligned = false;// true the initial tilt alignment has been calculated
float _ahrs_accel_fusion_gain; // gain from accel vector tilt error to rate gyro correction used by AHRS calculation
Vector3f _ahrs_accel; // low pass filtered body frame specific force vector used by AHRS calculation (m/s/s)
float _ahrs_accel_norm; // length of _ahrs_accel specific force vector used by AHRS calculation (m/s/s)
// calculate the gain from gravity vector misalingment to tilt correction to be used by all AHRS filters
void ahrsCalcAccelGain();
// update specified AHRS rotation matrix using IMU and optionally true airspeed data
void ahrsPredict(const uint8_t model_index);
// align all AHRS roll and pitch orientations using IMU delta velocity vector
void ahrsAlignTilt();
// align all AHRS yaw orientations to initial values
void ahrsAlignYaw();
// Efficient propagation of a delta angle in body frame applied to the body to earth frame rotation matrix
Matrix3f ahrsPredictRotMat(Matrix3f &R, Vector3f &g);
// Declarations used by a bank of N_MODELS_EKFGSF EKFs
struct _ekf_gsf_struct{
matrix::Vector3f X; // Vel North (m/s), Vel East (m/s), yaw (rad)s
matrix::SquareMatrix<float, 3> P; // covariance matrix
float W = 0.0f; // weighting
matrix::SquareMatrix<float, 2> S; // innovation covariance matrix
matrix::SquareMatrix<float, 2> S_inverse; // inverse of the innovation covariance matrix
float S_det_inverse; // inverse of the innovation covariance matrix determinant
matrix::Vector2f innov; // Velocity N,E innovation (m/s)
};
_ekf_gsf_struct _ekf_gsf[N_MODELS_EKFGSF];
bool _vel_data_updated; // true when velocity data has been updated
bool _run_ekf_gsf; // true when operating condition is suitable for to run the GSF and EKF models and fuse velocity data
Vector2f _vel_NE; // NE velocity observations (m/s)
float _vel_accuracy; // 1-sigma accuracy of velocity observations (m/s)
bool _ekf_gsf_vel_fuse_started; // true when the EKF's have started fusing velocity data and the prediction and update processing is active
// initialise states and covariance data for the GSF and EKF filters
void initialiseEKFGSF();
// predict state and covariance for the specified EKF using inertial data
void predictEKF(const uint8_t model_index);
// update state and covariance for the specified EKF using a NE velocity measurement
// return false if update failed
bool updateEKF(const uint8_t model_index);
inline float sq(float x) { return x * x; };
// converts Tait-Bryan 312 sequence of rotations from frame 1 to frame 2
// to the corresponding rotation matrix that rotates from frame 2 to frame 1
// rot312(0) - First rotation is a RH rotation about the Z axis (rad)
// rot312(1) - Second rotation is a RH rotation about the X axis (rad)
// rot312(2) - Third rotation is a RH rotation about the Y axis (rad)
// See http://www.atacolorado.com/eulersequences.doc
Dcmf taitBryan312ToRotMat(const Vector3f &rot312);
// Declarations used by the Gaussian Sum Filter (GSF) that combines the individual EKF yaw estimates
float _gsf_yaw; // yaw estimate (rad)
float _gsf_yaw_variance; // variance of yaw estimate (rad^2)
// return the probability of the state estimate for the specified EKF assuming a gaussian error distribution
float gaussianDensity(const uint8_t model_index) const;
// update the inverse of the innovation covariance matrix
void updateInnovCovMatInv(const uint8_t model_index);
};

5
EKF/common.h
View File

@ -354,6 +354,11 @@ struct parameters {
// compute synthetic magnetomter Z value if possible
int32_t synthesize_mag_z{0};
int32_t check_mag_strength{0};
// Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value
float EKFGSF_tas_default{15.0f}; ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
unsigned EKFGSF_reset_delay{1000000}; ///< Number of uSec of bad innovations on main filter inpost takeoff phase before yaw is reset to EKF-GSF value
float EKFGSF_yaw_err_max{0.2f}; ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad)
};
struct stateSample {

35
EKF/control.cpp
View File

@ -357,9 +357,8 @@ void Ekf::controlExternalVisionFusion()
}
// correct velocity for offset relative to IMU
const Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt);
const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
const Vector3f vel_offset_body = ang_rate % pos_offset_body;
const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
vel_aligned -= vel_offset_earth;
@ -657,7 +656,34 @@ void Ekf::controlGpsFusion()
// We haven't had an absolute position fix for a longer time so need to do something
do_reset = do_reset || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
if (do_reset) {
// A reset to the EKF-GSF estimate can be performed after a recent takeoff which will enable
// recovery from a bad magnetometer or field estimate.
// This special case reset can also be requested externally.
// The minimum time interval between resets to the EKF-GSF estimate must be limited to
// allow the EKF-GSF time to improve its estimate if the first reset was not successful.
const bool stopped_following_gps_velocity = isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) &&
(_time_last_hor_vel_fuse > _time_last_on_ground_us);
if (!_control_status.flags.in_air) {
_time_last_on_ground_us = _time_last_imu;
}
const bool recent_takeoff = _control_status.flags.in_air && !isTimedOut(_time_last_on_ground_us, 30000000);
const bool reset_yaw_to_EKFGSF = (do_reset || _do_emergency_yaw_reset || stopped_following_gps_velocity) &&
recent_takeoff &&
isTimedOut(_emergency_yaw_reset_time, 5000000);
if (reset_yaw_to_EKFGSF) {
if (resetYawToEKFGSF()) {
_emergency_yaw_reset_time = _time_last_imu;
_do_emergency_yaw_reset = false;
// Reset the timeout counters
_time_last_hor_pos_fuse = _time_last_imu;
_time_last_delpos_fuse = _time_last_imu;
_time_last_hor_vel_fuse = _time_last_imu;
_time_last_of_fuse = _time_last_imu;
}
} else if (do_reset) {
// use GPS velocity data to check and correct yaw angle if a FW vehicle
if (_control_status.flags.fixed_wing && _control_status.flags.in_air) {
// if flying a fixed wing aircraft, do a complete reset that includes yaw
@ -686,9 +712,8 @@ void Ekf::controlGpsFusion()
Vector3f gps_pos_obs_var;
// correct velocity for offset relative to IMU
const Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt);
const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
const Vector3f vel_offset_body = ang_rate % pos_offset_body;
const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
_gps_sample_delayed.vel -= vel_offset_earth;

13
EKF/ekf.cpp
View File

@ -83,6 +83,8 @@ void Ekf::reset()
_dt_ekf_avg = FILTER_UPDATE_PERIOD_S;
_ang_rate_delayed_raw.zero();
_fault_status.value = 0;
_innov_check_fail_status.value = 0;
@ -116,6 +118,9 @@ bool Ekf::update()
runTerrainEstimator();
updated = true;
// run EKF-GSF yaw estimator
runYawEKFGSF();
}
// the output observer always runs
@ -291,6 +296,14 @@ void Ekf::predictState()
// filter and limit input between -50% and +100% of nominal value
input = math::constrain(input, 0.5f * FILTER_UPDATE_PERIOD_S, 2.0f * FILTER_UPDATE_PERIOD_S);
_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input;
// some calculations elsewhere in code require a raw angular rate vector so calculate here to avoid duplication
// protect angainst possible small timesteps resulting from timing slip on previous frame that can drive spikes into the rate
// due to insufficient averaging
if (_imu_sample_delayed.delta_ang_dt > 0.25f * FILTER_UPDATE_PERIOD_S) {
_ang_rate_delayed_raw = _imu_sample_delayed.delta_ang / _imu_sample_delayed.delta_ang_dt;
}
}
/*

41
EKF/ekf.h
View File

@ -284,6 +284,17 @@ public:
// set minimum continuous period without GPS fail required to mark a healthy GPS status
void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; }
// get solution data from the EKF-GSF emergency yaw estimator
// returns false when data is not available
bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) override;
// Request the EKF reset the yaw to the estimate from the internal EKF-GSF filter
// and reset the velocity and position states to the GPS. This will cause the EKF
// to ignore the magnetomer for the remainder of flight.
// This should only be used as a last resort before activating a loss of navigation failsafe
// The counter must be incremented for each new reset request
void requestEmergencyNavReset(uint8_t counter) override;
private:
static constexpr uint8_t _k_num_states{24}; ///< number of EKF states
@ -304,6 +315,8 @@ private:
float _dt_ekf_avg{FILTER_UPDATE_PERIOD_S}; ///< average update rate of the ekf
float _dt_update{0.01f}; ///< delta time since last ekf update. This time can be used for filters which run at the same rate as the Ekf::update() function. (sec)
Vector3f _ang_rate_delayed_raw; ///< uncorrected angular rate vector at fusion time horizon (rad/sec)
stateSample _state{}; ///< state struct of the ekf running at the delayed time horizon
bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised
@ -815,4 +828,32 @@ private:
void setVelPosFaultStatus(const int index, const bool status);
// reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch
// yaw : Euler yaw angle (rad)
// yaw_variance : yaw error variance (rad^2)
// update_buffer : true if the state change should be also applied to the output observer buffer
void resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer);
// converts Tait-Bryan 312 sequence of rotations from frame 1 to frame 2
// to the corresponding rotation matrix that rotates from frame 2 to frame 1
// rot312(0) - First rotation is a RH rotation about the Z axis (rad)
// rot312(1) - Second rotation is a RH rotation about the X axis (rad)
// rot312(2) - Third rotation is a RH rotation about the Y axis (rad)
// See http://www.atacolorado.com/eulersequences.doc
Dcmf taitBryan312ToRotMat(const Vector3f &rot312);
// Declarations used to control use of the EKF-GSF yaw estimator
int64_t _emergency_yaw_reset_time{0}; ///< timestamp of last emergency yaw reset (uSec)
uint64_t _time_last_on_ground_us{0}; ///< last tine we were on the ground (uSec)
uint8_t _yaw_extreset_counter{0}; // number of external emergency yaw reset requests
bool _do_emergency_yaw_reset{false}; // true when an emergency yaw reset has been requested
// Call once per _imu_sample_delayed update after all main EKF data fusion oeprations have been completed
void runYawEKFGSF();
// Resets the main Nav EKf yaw to the esitmator from the EKF-GSF yaw estimator
// Returns true if the reset was successful
bool resetYawToEKFGSF();
};

403
EKF/ekf_helper.cpp
View File

@ -426,56 +426,43 @@ bool Ekf::realignYawGPS()
_control_status.flags.mag_fault = true;
}
// save a copy of the quaternion state for later use in calculating the amount of reset change
const Quatf quat_before_reset = _state.quat_nominal;
// update transformation matrix from body to world frame using the current state estimate
_R_to_earth = Dcmf(_state.quat_nominal);
// get quaternion from existing filter states and calculate roll, pitch and yaw angles
Eulerf euler321(_state.quat_nominal);
// apply yaw correction
// calculate new yaw estimate
float yaw_new;
if (!_control_status.flags.mag_aligned_in_flight) {
// This is our first flight alignment so we can assume that the recent change in velocity has occurred due to a
// forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error
euler321(2) += courseYawError;
Eulerf euler321(_state.quat_nominal);
yaw_new = euler321(2) + courseYawError;
_control_status.flags.mag_aligned_in_flight = true;
} else if (_control_status.flags.wind) {
// we have previously aligned yaw in-flight and have wind estimates so set the yaw such that the vehicle nose is
// aligned with the wind relative GPS velocity vector
euler321(2) = atan2f((_gps_sample_delayed.vel(1) - _state.wind_vel(1)),
yaw_new = atan2f((_gps_sample_delayed.vel(1) - _state.wind_vel(1)),
(_gps_sample_delayed.vel(0) - _state.wind_vel(0)));
} else {
// we don't have wind estimates, so align yaw to the GPS velocity vector
euler321(2) = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
yaw_new = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
}
// calculate new filter quaternion states using corrected yaw angle
_state.quat_nominal = Quatf(euler321);
_R_to_earth = Dcmf(_state.quat_nominal);
uncorrelateQuatFromOtherStates();
// If heading was bad, then we also need to reset the velocity and position states
_velpos_reset_request = badMagYaw;
// 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);
float yaw_variance_new = sq(asinf(sineYawError));
// adjust the quaternion covariances estimated yaw error
increaseQuatYawErrVariance(sq(asinf(sineYawError)));
// Apply updated yaw and yaw variance to states and covariances
resetQuatStateYaw(yaw_new, yaw_variance_new, true);
// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
// Use the last magnetometer measurements to reset the field states
_state.mag_B.zero();
_R_to_earth = Dcmf(_state.quat_nominal);
_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
// reset the corresponding rows and columns in the covariance matrix and set the
// variances on the magnetic field states to the measurement variance
clearMagCov();
if (_control_status.flags.mag_3D) {
for (uint8_t index = 16; index <= 21; index ++) {
P(index,index) = sq(_params.mag_noise);
@ -488,20 +475,8 @@ bool Ekf::realignYawGPS()
// record the start time for the magnetic field alignment
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
// 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
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
_output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
}
// 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 * _output_new.quat_nominal;
// capture the reset event
_state_reset_status.quat_counter++;
// If heading was bad, then we also need to reset the velocity and position states
_velpos_reset_request = badMagYaw;
return true;
@ -549,126 +524,61 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
return false;
}
// save a copy of the quaternion state for later use in calculating the amount of reset change
const Quatf quat_before_reset = _state.quat_nominal;
Quatf quat_after_reset = _state.quat_nominal;
// calculate the observed yaw angle and yaw variance
float yaw_new;
float yaw_new_variance = 0.0f;
if (_control_status.flags.ev_yaw) {
// convert the observed quaternion to a rotation matrix
const Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
// update transformation matrix from body to world frame using the current estimate
_R_to_earth = Dcmf(_state.quat_nominal);
// calculate the yaw angle for a 312 sequence
yaw_new = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0));
// calculate the initial quaternion
// determine if a 321 or 312 Euler sequence is best
if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
// use a 321 sequence
// rotate the magnetometer measurement into earth frame
Eulerf euler321(_state.quat_nominal);
// Set the yaw angle to zero and calculate the rotation matrix from body to earth frame
euler321(2) = 0.0f;
// calculate the observed yaw angle
if (_control_status.flags.ev_yaw) {
// convert the observed quaternion to a rotation matrix
const Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
// calculate the yaw angle for a 312 sequence
euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
const Dcmf R_to_earth(euler321);
// rotate the magnetometer measurements into earth frame using a zero yaw angle
const Vector3f mag_earth_pred = R_to_earth * mag_init;
// the angle of the projection onto the horizontal gives the yaw angle
euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) {
// we are operating without knowing the earth frame yaw angle
return true;
} else {
// there is no yaw observation
return false;
if (increase_yaw_var) {
yaw_new_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));
}
// calculate initial quaternion states for the ekf
// we don't change the output attitude to avoid jumps
quat_after_reset = Quatf(euler321);
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Dcmf R_to_earth;
if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
// rolled more than pitched so use 321 rotation order
Eulerf euler321(_state.quat_nominal);
euler321(2) = 0.0f;
R_to_earth = Dcmf(euler321);
} else {
// pitched more than rolled so use 312 rotation order
Vector3f rotVec312;
rotVec312(0) = 0.0f; // first rotation (yaw)
rotVec312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
rotVec312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch)
R_to_earth = taitBryan312ToRotMat(rotVec312);
}
// the angle of the projection onto the horizontal gives the yaw angle
const Vector3f mag_earth_pred = R_to_earth * mag_init;
yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
if (increase_yaw_var) {
yaw_new_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
}
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) {
// we are operating without knowing the earth frame yaw angle
return true;
} else {
// use a 312 sequence
// 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)
// Set the first rotation (yaw) to zero and calculate the rotation matrix from body to earth frame
euler312(0) = 0.0f;
// 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));
Dcmf R_to_earth;
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;
// calculate the observed yaw angle
if (_control_status.flags.ev_yaw) {
// convert the observed quaternion to a rotation matrix
const Dcmf R_to_earth_ev(_ev_sample_delayed.quat);
// calculate the yaw angle for a 312 sequence
euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
const Vector3f mag_earth_pred = R_to_earth * mag_init;
// the angle of the projection onto the horizontal gives the yaw angle
euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) {
// we are operating without knowing the earth frame yaw angle
return true;
} else {
// there is no yaw observation
return false;
}
// re-calculate the rotation matrix using the updated yaw angle
s0 = sinf(euler312(0));
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;
// calculate initial quaternion states for the ekf
// we don't change the output attitude to avoid jumps
quat_after_reset = Quatf(R_to_earth);
// there is no yaw observation
return false;
}
// update quaternion states and corresponding covarainces
resetQuatStateYaw(yaw_new, yaw_new_variance, update_buffer);
// set the earth magnetic field states using the updated rotation
const Dcmf R_to_earth_after(quat_after_reset);
_state.mag_I = R_to_earth_after * mag_init;
_state.mag_I = _R_to_earth * mag_init;
// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
clearMagCov();
@ -685,42 +595,6 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
// record the time for the magnetic field alignment event
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
// calculate the amount that the quaternion has changed by
const Quatf q_error((quat_after_reset * quat_before_reset.inversed()).normalized());
// update quaternion states
_state.quat_nominal = quat_after_reset;
_R_to_earth = Dcmf(_state.quat_nominal);
uncorrelateQuatFromOtherStates();
// record the state change
_state_reset_status.quat_change = q_error;
if (increase_yaw_var) {
// update the yaw angle variance using the variance of the measurement
if (_control_status.flags.ev_yaw) {
// using error estimate from external vision data
increaseQuatYawErrVariance(fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f)));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// using magnetic heading tuning parameter
increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));
}
}
if (update_buffer) {
// add the reset amount to the output observer buffered data
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
_output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
}
// 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 * _output_new.quat_nominal;
}
// capture the reset event
_state_reset_status.quat_counter++;
return true;
}
@ -1370,8 +1244,8 @@ void Ekf::fuse(float *K, float innovation)
void Ekf::uncorrelateQuatFromOtherStates()
{
P.slice<_k_num_states - 4, 4>(4, 0) = 0.f;
P.slice<4, _k_num_states - 4>(0, 4) = 0.f;
P.slice<_k_num_states - 4, 4>(4, 0) = 0.f;
P.slice<4, _k_num_states - 4>(0, 4) = 0.f;
}
bool Ekf::global_position_is_valid()
@ -1819,3 +1693,148 @@ void Ekf::stopFlowFusion()
memset(_flow_innov_var,0.0f,sizeof(_flow_innov_var));
memset(&_optflow_test_ratio,0.0f,sizeof(_optflow_test_ratio));
}
void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
{
// save a copy of the quaternion state for later use in calculating the amount of reset change
const Quatf quat_before_reset = _state.quat_nominal;
// update transformation matrix from body to world frame using the current estimate
_R_to_earth = Dcmf(_state.quat_nominal);
// update the rotation matrix using the new yaw value
// determine if a 321 or 312 Euler sequence is best
if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
// use a 321 sequence
Eulerf euler321(_R_to_earth);
euler321(2) = yaw;
_R_to_earth = Dcmf(euler321);
} else {
// Calculate the 312 Tait-Bryan rotation sequence that rotates from earth to body frame
// We use a 312 sequence as an alternate when there is more pitch tilt than roll tilt
// to avoid gimbal lock
Vector3f rot312;
rot312(0) = yaw;
rot312(1) = asinf(_R_to_earth(2, 1));
rot312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2));
_R_to_earth = taitBryan312ToRotMat(rot312);
}
// calculate the amount that the quaternion has changed by
const Quatf quat_after_reset = _R_to_earth;
Quatf q_error = quat_after_reset * quat_before_reset.inversed();
q_error.normalize();
// update quaternion states
_state.quat_nominal = quat_after_reset;
uncorrelateQuatFromOtherStates();
// record the state change
_state_reset_status.quat_change = q_error;
// update the yaw angle variance
if (yaw_variance > FLT_EPSILON) {
increaseQuatYawErrVariance(yaw_variance);
}
// add the reset amount to the output observer buffered data
if (update_buffer) {
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
_output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
}
// 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 * _output_new.quat_nominal;
}
// capture the reset event
_state_reset_status.quat_counter++;
}
// Reset main nav filter yaw to value from EKF-GSF and reset velocity and position to GPS
bool Ekf::resetYawToEKFGSF()
{
// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity
// data and the yaw estimate has converged
float new_yaw, new_yaw_variance;
if (yawEstimator.getYawData(&new_yaw, &new_yaw_variance) && new_yaw_variance < sq(_params.EKFGSF_yaw_err_max)) {
resetQuatStateYaw(new_yaw, new_yaw_variance, true);
// reset velocity and position states to GPS - if yaw is fixed then the filter should start to operate correctly
resetVelocity();
resetPosition();
// stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around
// and cause another navigation failure
_control_status.flags.mag_fault = true;
// record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
ECL_INFO_TIMESTAMPED("Emergency yaw reset - magnetometer use stopped");
return true;
}
return false;
}
void Ekf::requestEmergencyNavReset(uint8_t counter)
{
if (counter > _yaw_extreset_counter) {
_yaw_extreset_counter = counter;
_do_emergency_yaw_reset = true;
}
}
bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
{
return yawEstimator.getLogData(yaw_composite,yaw_variance,yaw,innov_VN,innov_VE,weight);
}
Dcmf Ekf::taitBryan312ToRotMat(const Vector3f &rot312)
{
// Calculate the frame2 to frame 1 rotation matrix from a 312 Tait-Bryan rotation sequence
const float c2 = cosf(rot312(2)); // third rotation is pitch
const float s2 = sinf(rot312(2));
const float s1 = sinf(rot312(1)); // second rotation is roll
const float c1 = cosf(rot312(1));
const float s0 = sinf(rot312(0)); // first rotation is yaw
const float c0 = cosf(rot312(0));
Dcmf R;
R(0, 0) = c0 * c2 - s0 * s1 * s2;
R(1, 1) = c0 * c1;
R(2, 2) = c2 * c1;
R(0, 1) = -c1 * s0;
R(0, 2) = s2 * c0 + c2 * s1 * s0;
R(1, 0) = c2 * s0 + s2 * s1 * c0;
R(1, 2) = s0 * s2 - s1 * c0 * c2;
R(2, 0) = -s2 * c1;
R(2, 1) = s1;
return R;
}
void Ekf::runYawEKFGSF()
{
float TAS;
if (isTimedOut(_airspeed_sample_delayed.time_us, 1000000) && _control_status.flags.fixed_wing) {
TAS = _params.EKFGSF_tas_default;
} else {
TAS = _airspeed_sample_delayed.true_airspeed;
}
yawEstimator.update(_imu_sample_delayed.delta_ang, _imu_sample_delayed.delta_vel, _imu_sample_delayed.delta_ang_dt, _imu_sample_delayed.delta_vel_dt, _control_status.flags.in_air, TAS);
// basic sanity check on GPS velocity data
if (_gps_data_ready && _gps_sample_delayed.vacc > FLT_EPSILON && isfinite(_gps_sample_delayed.vel(0)) && isfinite(_gps_sample_delayed.vel(1))) {
Vector2f vel_NE = Vector2f(_gps_sample_delayed.vel(0),_gps_sample_delayed.vel(1));
yawEstimator.setVelocity(vel_NE, _gps_sample_delayed.vacc);
}
}

11
EKF/estimator_interface.h
View File

@ -46,6 +46,7 @@
#include "RingBuffer.h"
#include "AlphaFilter.hpp"
#include "imu_down_sampler.hpp"
#include "EKFGSF_yaw.h"
#include <geo/geo.h>
#include <matrix/math.hpp>
@ -414,6 +415,13 @@ public:
static constexpr unsigned FILTER_UPDATE_PERIOD_MS{8}; // ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta
static constexpr float FILTER_UPDATE_PERIOD_S{FILTER_UPDATE_PERIOD_MS * 0.001f};
// request the EKF reset the yaw to the estimate from the internal EKF-GSF filter
// argment should be incremented only when a new reset is required
virtual void requestEmergencyNavReset(uint8_t counter) = 0;
// get ekf-gsf debug data
virtual bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) = 0;
protected:
parameters _params; // filter parameters
@ -538,6 +546,9 @@ protected:
RingBuffer<dragSample> _drag_buffer;
RingBuffer<auxVelSample> _auxvel_buffer;
// yaw estimator instance
EKFGSF_yaw yawEstimator;
// observation buffer final allocation failed
bool _gps_buffer_fail{false};
bool _mag_buffer_fail{false};

42
EKF/mag_fusion.cpp
View File

@ -624,46 +624,20 @@ void Ekf::fuseHeading()
}
/* Calculate the 312 sequence euler angles that rotate from earth to body frame
* Derived from https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m
* Body to nav frame transformation using a yaw-roll-pitch rotation sequence is given by:
*
[ cos(pitch)*cos(yaw) - sin(pitch)*sin(roll)*sin(yaw), -cos(roll)*sin(yaw), cos(yaw)*sin(pitch) + cos(pitch)*sin(roll)*sin(yaw)]
[ cos(pitch)*sin(yaw) + cos(yaw)*sin(pitch)*sin(roll), cos(roll)*cos(yaw), sin(pitch)*sin(yaw) - cos(pitch)*cos(yaw)*sin(roll)]
[ -cos(roll)*sin(pitch), sin(roll), cos(pitch)*cos(roll)]
*/
float yaw = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)); // first rotation (yaw)
const float roll = asinf(_R_to_earth(2, 1)); // second rotation (roll)
const float pitch = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch)
predicted_hdg = yaw; // we will need the predicted heading to calculate the innovation
// Use a Tait-Bryan 312 rotation sequence
Vector3f rotVec312;
predicted_hdg = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1));
rotVec312(0) = 0.0f; // first rotation (yaw) set to zero for alter use when rotating the mag field into earth frame
rotVec312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
rotVec312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch)
// calculate the observed yaw angle
if (_control_status.flags.mag_hdg) {
// Set the first rotation (yaw) to zero and rotate the measurements into earth frame
yaw = 0.0f;
// Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence
// Equations from Tbn_312.c produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m
Dcmf R_to_earth;
float sy = sinf(yaw);
float cy = cosf(yaw);
float sp = sinf(pitch);
float cp = cosf(pitch);
float sr = sinf(roll);
float cr = cosf(roll);
R_to_earth(0,0) = cy*cp-sy*sp*sr;
R_to_earth(0,1) = -sy*cr;
R_to_earth(0,2) = cy*sp+sy*cp*sr;
R_to_earth(1,0) = sy*cp+cy*sp*sr;
R_to_earth(1,1) = cy*cr;
R_to_earth(1,2) = sy*sp-cy*cp*sr;
R_to_earth(2,0) = -sp*cr;
R_to_earth(2,1) = sr;
R_to_earth(2,2) = cp*cr;
// with yaw angle set to to zero
const Dcmf R_to_earth = taitBryan312ToRotMat(rotVec312);
// rotate the magnetometer measurements into earth frame using a zero yaw angle
if (_control_status.flags.mag_3D) {
// don't apply bias corrections if we are learning them