Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: allow for double EKF build

This commit is contained in:
Andrew Tridgell 2021-05-04 21:12:23 +10:00 committed by Randy Mackay
parent 6484a9666c
commit d31ca6ca8c
6 changed files with 229 additions and 216 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF/AP_NavEKF_core_common.cpp
View File

@ -31,9 +31,9 @@ void NavEKF_core_common::fill_scratch_variables(void)
// SITL where they are used without initialisation. These are all // SITL where they are used without initialisation. These are all
// supposed to be scratch variables that are not used between // supposed to be scratch variables that are not used between
// iterations // iterations
fill_nanf(&KH[0][0], sizeof(KH)/sizeof(float)); fill_nanf(&KH[0][0], sizeof(KH)/sizeof(ftype));
fill_nanf(&KHP[0][0], sizeof(KHP)/sizeof(float)); fill_nanf(&KHP[0][0], sizeof(KHP)/sizeof(ftype));
fill_nanf(&nextP[0][0], sizeof(nextP)/sizeof(float)); fill_nanf(&nextP[0][0], sizeof(nextP)/sizeof(ftype));
fill_nanf(&Kfusion[0], sizeof(Kfusion)/sizeof(float)); fill_nanf(&Kfusion[0], sizeof(Kfusion)/sizeof(ftype));
#endif #endif
} }

13
libraries/AP_NavEKF/AP_NavEKF_core_common.h
View File

@ -19,6 +19,7 @@
#include <stdint.h> #include <stdint.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <AP_Math/vectorN.h> #include <AP_Math/vectorN.h>
#include "AP_Nav_Common.h"
/* /*
this declares a common parent class for AP_NavEKF2 and this declares a common parent class for AP_NavEKF2 and
@ -32,7 +33,6 @@
*/ */
class NavEKF_core_common { class NavEKF_core_common {
public: public:
typedef float ftype;
#if MATH_CHECK_INDEXES #if MATH_CHECK_INDEXES
typedef VectorN<ftype,28> Vector28; typedef VectorN<ftype,28> Vector28;
typedef VectorN<VectorN<ftype,24>,24> Matrix24; typedef VectorN<VectorN<ftype,24>,24> Matrix24;
@ -49,4 +49,15 @@ protected:
// fill all the common scratch variables with NaN on SITL // fill all the common scratch variables with NaN on SITL
void fill_scratch_variables(void); void fill_scratch_variables(void);
// zero part of an array
static void zero_range(ftype *v, uint8_t n1, uint8_t n2) {
memset(&v[n1], 0, sizeof(ftype)*(1+(n2-n1)));
}
}; };
#if HAL_WITH_EKF_DOUBLE
// stack frames are larger with double EKF
#pragma GCC diagnostic error "-Wframe-larger-than=2100"
#endif

1
libraries/AP_NavEKF/AP_Nav_Common.h
View File

@ -17,6 +17,7 @@
#pragma once #pragma once
#include <stdint.h> #include <stdint.h>
#include <AP_Math/AP_Math.h>
union nav_filter_status { union nav_filter_status {
struct { struct {

4
libraries/AP_NavEKF/EKFGSF_Logging.cpp
View File

@ -2,6 +2,8 @@
#include <AP_Logger/AP_Logger.h> #include <AP_Logger/AP_Logger.h>
#pragma GCC diagnostic ignored "-Wnarrowing"
void EKFGSF_yaw::Log_Write(uint64_t time_us, LogMessages id0, LogMessages id1, uint8_t core_index) void EKFGSF_yaw::Log_Write(uint64_t time_us, LogMessages id0, LogMessages id1, uint8_t core_index)
{ {
if (!vel_fuse_running) { if (!vel_fuse_running) {
@ -15,7 +17,7 @@ void EKFGSF_yaw::Log_Write(uint64_t time_us, LogMessages id0, LogMessages id1, u
time_us : time_us, time_us : time_us,
core : core_index, core : core_index,
yaw_composite : GSF.yaw, yaw_composite : GSF.yaw,
yaw_composite_variance : sqrtf(MAX(GSF.yaw_variance, 0.0f)), yaw_composite_variance : sqrtF(MAX(GSF.yaw_variance, 0.0f)),
yaw0 : EKF[0].X[2], yaw0 : EKF[0].X[2],
yaw1 : EKF[1].X[2], yaw1 : EKF[1].X[2],
yaw2 : EKF[2].X[2], yaw2 : EKF[2].X[2],

342
libraries/AP_NavEKF/EKFGSF_yaw.cpp
View File

@ -28,12 +28,12 @@
EKFGSF_yaw::EKFGSF_yaw() {}; EKFGSF_yaw::EKFGSF_yaw() {};
void EKFGSF_yaw::update(const Vector3f &delAng, void EKFGSF_yaw::update(const Vector3F &delAng,
const Vector3f &delVel, const Vector3F &delVel,
const float delAngDT, const ftype delAngDT,
const float delVelDT, const ftype delVelDT,
bool runEKF, bool runEKF,
float TAS) ftype TAS)
{ {
// copy to class variables // copy to class variables
@ -46,15 +46,15 @@ void EKFGSF_yaw::update(const Vector3f &delAng,
// Calculate a low pass filtered acceleration vector that will be used to keep the AHRS tilt aligned // Calculate a low pass filtered acceleration vector that will be used to keep the AHRS tilt aligned
// The time constant of the filter is a fixed ratio relative to the time constant of the AHRS tilt correction loop // The time constant of the filter is a fixed ratio relative to the time constant of the AHRS tilt correction loop
const float filter_coef = fminf(EKFGSF_accelFiltRatio * delVelDT * EKFGSF_tiltGain, 1.0f); const ftype filter_coef = fminF(EKFGSF_accelFiltRatio * delVelDT * EKFGSF_tiltGain, 1.0f);
const Vector3f accel = delVel / fmaxf(delVelDT, 0.001f); const Vector3F accel = delVel / fmaxF(delVelDT, 0.001f);
ahrs_accel = ahrs_accel * (1.0f - filter_coef) + accel * filter_coef; ahrs_accel = ahrs_accel * (1.0f - filter_coef) + accel * filter_coef;
// Iniitialise states and only when acceleration is close to 1g to prevent vehicle movement casuing a large initial tilt error // Iniitialise states and only when acceleration is close to 1g to prevent vehicle movement casuing a large initial tilt error
if (!ahrs_tilt_aligned) { if (!ahrs_tilt_aligned) {
const float accel_norm_sq = accel.length_squared(); const ftype accel_norm_sq = accel.length_squared();
const float upper_accel_limit = GRAVITY_MSS * 1.1f; const ftype upper_accel_limit = GRAVITY_MSS * 1.1f;
const float lower_accel_limit = GRAVITY_MSS * 0.9f; const ftype lower_accel_limit = GRAVITY_MSS * 0.9f;
const bool ok_to_align = ((accel_norm_sq > lower_accel_limit * lower_accel_limit && const bool ok_to_align = ((accel_norm_sq > lower_accel_limit * lower_accel_limit &&
accel_norm_sq < upper_accel_limit * upper_accel_limit)); accel_norm_sq < upper_accel_limit * upper_accel_limit));
if (ok_to_align) { if (ok_to_align) {
@ -71,7 +71,7 @@ void EKFGSF_yaw::update(const Vector3f &delAng,
// Calculate AHRS acceleration fusion gain using a quadratic weighting function that is unity at 1g // Calculate AHRS acceleration fusion gain using a quadratic weighting function that is unity at 1g
// and zero at the min and max g limits. This reduces the effect of large g transients on the attitude // and zero at the min and max g limits. This reduces the effect of large g transients on the attitude
// esitmates. // esitmates.
float EKFGSF_ahrs_ng = ahrs_accel_norm / GRAVITY_MSS; ftype EKFGSF_ahrs_ng = ahrs_accel_norm / GRAVITY_MSS;
if (EKFGSF_ahrs_ng > 1.0f) { if (EKFGSF_ahrs_ng > 1.0f) {
if (is_positive(true_airspeed)) { if (is_positive(true_airspeed)) {
// When flying in fixed wing mode we need to allow for more positive g due to coordinated turns // When flying in fixed wing mode we need to allow for more positive g due to coordinated turns
@ -104,18 +104,18 @@ void EKFGSF_yaw::update(const Vector3f &delAng,
// Calculate a composite yaw as a weighted average of the states for each model. // Calculate a composite yaw 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 // 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. // equal to the weighting value before it is summed.
Vector2f yaw_vector = {}; Vector2F yaw_vector = {};
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) { for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
yaw_vector[0] += GSF.weights[mdl_idx] * cosf(EKF[mdl_idx].X[2]); yaw_vector[0] += GSF.weights[mdl_idx] * cosF(EKF[mdl_idx].X[2]);
yaw_vector[1] += GSF.weights[mdl_idx] * sinf(EKF[mdl_idx].X[2]); yaw_vector[1] += GSF.weights[mdl_idx] * sinF(EKF[mdl_idx].X[2]);
} }
GSF.yaw = atan2f(yaw_vector[1],yaw_vector[0]); GSF.yaw = atan2F(yaw_vector[1],yaw_vector[0]);
// Example for future reference showing how a full GSF covariance matrix could be calculated if required // Example for future reference showing how a full GSF covariance matrix could be calculated if required
/* /*
memset(&GSF.P, 0, sizeof(GSF.P)); memset(&GSF.P, 0, sizeof(GSF.P));
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) { for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
float delta[3]; ftype delta[3];
for (uint8_t row = 0; row < 3; row++) { for (uint8_t row = 0; row < 3; row++) {
delta[row] = EKF[mdl_idx].X[row] - GSF.X[row]; delta[row] = EKF[mdl_idx].X[row] - GSF.X[row];
} }
@ -129,15 +129,15 @@ void EKFGSF_yaw::update(const Vector3f &delAng,
GSF.yaw_variance = 0.0f; GSF.yaw_variance = 0.0f;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) { for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
float yawDelta = wrap_PI(EKF[mdl_idx].X[2] - GSF.yaw); ftype yawDelta = wrap_PI(EKF[mdl_idx].X[2] - GSF.yaw);
GSF.yaw_variance += GSF.weights[mdl_idx] * (EKF[mdl_idx].P[2][2] + sq(yawDelta)); GSF.yaw_variance += GSF.weights[mdl_idx] * (EKF[mdl_idx].P[2][2] + sq(yawDelta));
} }
} }
void EKFGSF_yaw::fuseVelData(const Vector2f &vel, const float velAcc) void EKFGSF_yaw::fuseVelData(const Vector2F &vel, const ftype velAcc)
{ {
// convert reported accuracy to a variance, but limit lower value to protect algorithm stability // convert reported accuracy to a variance, but limit lower value to protect algorithm stability
const float velObsVar = sq(fmaxf(velAcc, 0.5f)); const ftype velObsVar = sq(fmaxF(velAcc, 0.5f));
// The 3-state EKF models only run when flying to avoid corrupted estimates due to operator handling and GPS interference // The 3-state EKF models only run when flying to avoid corrupted estimates due to operator handling and GPS interference
if (run_ekf_gsf) { if (run_ekf_gsf) {
@ -154,8 +154,8 @@ void EKFGSF_yaw::fuseVelData(const Vector2f &vel, const float velAcc)
alignYaw(); alignYaw();
vel_fuse_running = true; vel_fuse_running = true;
} else { } else {
float total_w = 0.0f; ftype total_w = 0.0f;
float newWeight[(uint8_t)N_MODELS_EKFGSF]; ftype newWeight[(uint8_t)N_MODELS_EKFGSF];
bool state_update_failed = false; bool state_update_failed = false;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) { for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
// Update states and covariances using GPS NE velocity measurements fused as direct state observations // Update states and covariances using GPS NE velocity measurements fused as direct state observations
@ -166,7 +166,7 @@ void EKFGSF_yaw::fuseVelData(const Vector2f &vel, const float velAcc)
if (!state_update_failed) { if (!state_update_failed) {
// Calculate weighting for each model assuming a normal error distribution // Calculate weighting for each model assuming a normal error distribution
const float min_weight = 1e-5f; const ftype min_weight = 1e-5f;
uint8_t n_clips = 0; uint8_t n_clips = 0;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) { for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
newWeight[mdl_idx] = gaussianDensity(mdl_idx) * GSF.weights[mdl_idx]; newWeight[mdl_idx] = gaussianDensity(mdl_idx) * GSF.weights[mdl_idx];
@ -180,7 +180,7 @@ void EKFGSF_yaw::fuseVelData(const Vector2f &vel, const float velAcc)
// Normalise the sum of weights to unity // Normalise the sum of weights to unity
// Reset the filters if all weights have underflowed due to excessive innovation variances // Reset the filters if all weights have underflowed due to excessive innovation variances
if (vel_fuse_running && n_clips < N_MODELS_EKFGSF) { if (vel_fuse_running && n_clips < N_MODELS_EKFGSF) {
float total_w_inv = 1.0f / total_w; ftype total_w_inv = 1.0f / total_w;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) { for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
GSF.weights[mdl_idx] = newWeight[mdl_idx] * total_w_inv; GSF.weights[mdl_idx] = newWeight[mdl_idx] * total_w_inv;
} }
@ -197,24 +197,24 @@ void EKFGSF_yaw::predictAHRS(const uint8_t mdl_idx)
// Generate attitude solution using simple complementary filter for the selected model // Generate attitude solution using simple complementary filter for the selected model
// Calculate 'k' unit vector of earth frame rotated into body frame // Calculate 'k' unit vector of earth frame rotated into body frame
const Vector3f k(AHRS[mdl_idx].R[2][0], AHRS[mdl_idx].R[2][1], AHRS[mdl_idx].R[2][2]); const Vector3F k(AHRS[mdl_idx].R[2][0], AHRS[mdl_idx].R[2][1], AHRS[mdl_idx].R[2][2]);
// Calculate angular rate vector in rad/sec averaged across last sample interval // Calculate angular rate vector in rad/sec averaged across last sample interval
Vector3f ang_rate_delayed_raw = delta_angle / angle_dt; Vector3F ang_rate_delayed_raw = delta_angle / angle_dt;
// 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). // 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 // During fixed wing flight, compensate for centripetal acceleration assuming coordinated turns and X axis forward
Vector3f tilt_error_gyro_correction = {}; // (rad/sec) Vector3F tilt_error_gyro_correction = {}; // (rad/sec)
if (accel_gain > 0.0f) { if (accel_gain > 0.0f) {
Vector3f accel = ahrs_accel; Vector3F accel = ahrs_accel;
if (is_positive(true_airspeed)) { if (is_positive(true_airspeed)) {
// Calculate centripetal acceleration in body frame from cross product of body rate and body frame airspeed vector // Calculate centripetal acceleration in body frame from cross product of body rate and body frame airspeed vector
// NOTE: this assumes X axis is aligned with airspeed vector // NOTE: this assumes X axis is aligned with airspeed vector
Vector3f centripetal_accel_vec_bf = Vector3f(0.0f, ang_rate_delayed_raw[2] * true_airspeed, - ang_rate_delayed_raw[1] * true_airspeed); Vector3F centripetal_accel_vec_bf = Vector3F(0.0f, ang_rate_delayed_raw[2] * true_airspeed, - ang_rate_delayed_raw[1] * true_airspeed);
// Correct measured accel for centripetal acceleration // Correct measured accel for centripetal acceleration
accel -= centripetal_accel_vec_bf; accel -= centripetal_accel_vec_bf;
@ -225,8 +225,8 @@ void EKFGSF_yaw::predictAHRS(const uint8_t mdl_idx)
} }
// Gyro bias estimation // Gyro bias estimation
const float gyro_bias_limit = radians(5.0f); const ftype gyro_bias_limit = radians(5.0f);
const float spinRate = ang_rate_delayed_raw.length(); const ftype spinRate = ang_rate_delayed_raw.length();
if (spinRate < 0.175f) { if (spinRate < 0.175f) {
AHRS[mdl_idx].gyro_bias -= tilt_error_gyro_correction * (EKFGSF_gyroBiasGain * angle_dt); AHRS[mdl_idx].gyro_bias -= tilt_error_gyro_correction * (EKFGSF_gyroBiasGain * angle_dt);
@ -236,7 +236,7 @@ void EKFGSF_yaw::predictAHRS(const uint8_t mdl_idx)
} }
// Calculate the corrected body frame rotation vector for the last sample interval and apply to the rotation matrix // Calculate the corrected body frame rotation vector for the last sample interval and apply to the rotation matrix
const Vector3f ahrs_delta_angle = delta_angle + (tilt_error_gyro_correction - AHRS[mdl_idx].gyro_bias) * angle_dt; const Vector3F ahrs_delta_angle = delta_angle + (tilt_error_gyro_correction - AHRS[mdl_idx].gyro_bias) * angle_dt;
AHRS[mdl_idx].R = updateRotMat(AHRS[mdl_idx].R, ahrs_delta_angle); AHRS[mdl_idx].R = updateRotMat(AHRS[mdl_idx].R, ahrs_delta_angle);
} }
@ -249,24 +249,24 @@ void EKFGSF_yaw::alignTilt()
// 2) The vehicle is not accelerating so all of the measured acceleration is due to gravity. // 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 // Calculate earth frame Down axis unit vector rotated into body frame
Vector3f down_in_bf = -delta_velocity; Vector3F down_in_bf = -delta_velocity;
down_in_bf.normalize(); down_in_bf.normalize();
// Calculate earth frame North axis unit vector rotated into body frame, orthogonal to 'down_in_bf' // Calculate earth frame North axis unit vector rotated into body frame, orthogonal to 'down_in_bf'
// * operator is overloaded to provide a dot product // * operator is overloaded to provide a dot product
const Vector3f i_vec_bf(1.0f,0.0f,0.0f); 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); Vector3F north_in_bf = i_vec_bf - down_in_bf * (i_vec_bf * down_in_bf);
north_in_bf.normalize(); north_in_bf.normalize();
// Calculate earth frame East axis unit vector rotated into body frame, orthogonal to 'down_in_bf' and 'north_in_bf' // 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 // % operator is overloaded to provide a cross product
const Vector3f east_in_bf = down_in_bf % north_in_bf; 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 // 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. // 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 // 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 to create the transpose. // frame are copied into corresponding rows instead to create the transpose.
Matrix3f R; Matrix3F R;
for (uint8_t col=0; col<3; col++) { for (uint8_t col=0; col<3; col++) {
R[0][col] = north_in_bf[col]; R[0][col] = north_in_bf[col];
R[1][col] = east_in_bf[col]; R[1][col] = east_in_bf[col];
@ -284,9 +284,9 @@ void EKFGSF_yaw::alignYaw()
{ {
// Align yaw angle for each model // Align yaw angle for each model
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) { for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
if (fabsf(AHRS[mdl_idx].R[2][0]) < fabsf(AHRS[mdl_idx].R[2][1])) { if (fabsF(AHRS[mdl_idx].R[2][0]) < fabsF(AHRS[mdl_idx].R[2][1])) {
// get the roll, pitch, yaw estimates from the rotation matrix using a 321 Tait-Bryan rotation sequence // get the roll, pitch, yaw estimates from the rotation matrix using a 321 Tait-Bryan rotation sequence
float roll,pitch,yaw; ftype roll,pitch,yaw;
AHRS[mdl_idx].R.to_euler(&roll, &pitch, &yaw); AHRS[mdl_idx].R.to_euler(&roll, &pitch, &yaw);
// set the yaw angle // set the yaw angle
@ -297,7 +297,7 @@ void EKFGSF_yaw::alignYaw()
} else { } else {
// Calculate the 312 Tait-Bryan rotation sequence that rotates from earth to body frame // Calculate the 312 Tait-Bryan rotation sequence that rotates from earth to body frame
Vector3f euler312 = AHRS[mdl_idx].R.to_euler312(); Vector3F euler312 = AHRS[mdl_idx].R.to_euler312();
euler312[2] = wrap_PI(EKF[mdl_idx].X[2]); // first rotation (yaw) taken from EKF model state euler312[2] = wrap_PI(EKF[mdl_idx].X[2]); // first rotation (yaw) taken from EKF model state
// update the body to earth frame rotation matrix // update the body to earth frame rotation matrix
@ -319,18 +319,18 @@ void EKFGSF_yaw::predict(const uint8_t mdl_idx)
} }
// Calculate the yaw state using a projection onto the horizontal that avoids gimbal lock // Calculate the yaw state using a projection onto the horizontal that avoids gimbal lock
if (fabsf(AHRS[mdl_idx].R[2][0]) < fabsf(AHRS[mdl_idx].R[2][1])) { if (fabsF(AHRS[mdl_idx].R[2][0]) < fabsF(AHRS[mdl_idx].R[2][1])) {
// use 321 Tait-Bryan rotation to define yaw state // use 321 Tait-Bryan rotation to define yaw state
EKF[mdl_idx].X[2] = atan2f(AHRS[mdl_idx].R[1][0], AHRS[mdl_idx].R[0][0]); EKF[mdl_idx].X[2] = atan2F(AHRS[mdl_idx].R[1][0], AHRS[mdl_idx].R[0][0]);
} else { } else {
// use 312 Tait-Bryan rotation to define yaw state // use 312 Tait-Bryan rotation to define yaw state
EKF[mdl_idx].X[2] = atan2f(-AHRS[mdl_idx].R[0][1], AHRS[mdl_idx].R[1][1]); // first rotation (yaw) EKF[mdl_idx].X[2] = atan2F(-AHRS[mdl_idx].R[0][1], AHRS[mdl_idx].R[1][1]); // first rotation (yaw)
} }
// calculate delta velocity in a horizontal front-right frame // calculate delta velocity in a horizontal front-right frame
const Vector3f del_vel_NED = AHRS[mdl_idx].R * delta_velocity; const Vector3F del_vel_NED = AHRS[mdl_idx].R * delta_velocity;
const float dvx = del_vel_NED[0] * cosf(EKF[mdl_idx].X[2]) + del_vel_NED[1] * sinf(EKF[mdl_idx].X[2]); const ftype dvx = del_vel_NED[0] * cosF(EKF[mdl_idx].X[2]) + del_vel_NED[1] * sinF(EKF[mdl_idx].X[2]);
const float dvy = - del_vel_NED[0] * sinf(EKF[mdl_idx].X[2]) + del_vel_NED[1] * cosf(EKF[mdl_idx].X[2]); const ftype dvy = - del_vel_NED[0] * sinF(EKF[mdl_idx].X[2]) + del_vel_NED[1] * cosF(EKF[mdl_idx].X[2]);
// sum delta velocities in earth frame: // sum delta velocities in earth frame:
EKF[mdl_idx].X[0] += del_vel_NED[0]; EKF[mdl_idx].X[0] += del_vel_NED[0];
@ -340,47 +340,47 @@ void EKFGSF_yaw::predict(const uint8_t mdl_idx)
// Local short variable name copies required for readability // Local short variable name copies required for readability
// Compiler might be smart enough to optimise these out // Compiler might be smart enough to optimise these out
const float P00 = EKF[mdl_idx].P[0][0]; const ftype P00 = EKF[mdl_idx].P[0][0];
const float P01 = EKF[mdl_idx].P[0][1]; const ftype P01 = EKF[mdl_idx].P[0][1];
const float P02 = EKF[mdl_idx].P[0][2]; const ftype P02 = EKF[mdl_idx].P[0][2];
const float P10 = EKF[mdl_idx].P[1][0]; const ftype P10 = EKF[mdl_idx].P[1][0];
const float P11 = EKF[mdl_idx].P[1][1]; const ftype P11 = EKF[mdl_idx].P[1][1];
const float P12 = EKF[mdl_idx].P[1][2]; const ftype P12 = EKF[mdl_idx].P[1][2];
const float P20 = EKF[mdl_idx].P[2][0]; const ftype P20 = EKF[mdl_idx].P[2][0];
const float P21 = EKF[mdl_idx].P[2][1]; const ftype P21 = EKF[mdl_idx].P[2][1];
const float P22 = EKF[mdl_idx].P[2][2]; const ftype P22 = EKF[mdl_idx].P[2][2];
// Use fixed values for delta velocity and delta angle process noise variances // Use fixed values for delta velocity and delta angle process noise variances
const float dvxVar = sq(EKFGSF_accelNoise * velocity_dt); // variance of forward delta velocity - (m/s)^2 const ftype dvxVar = sq(EKFGSF_accelNoise * velocity_dt); // variance of forward delta velocity - (m/s)^2
const float dvyVar = dvxVar; // variance of right delta velocity - (m/s)^2 const ftype dvyVar = dvxVar; // variance of right delta velocity - (m/s)^2
const float dazVar = sq(EKFGSF_gyroNoise * angle_dt); // variance of yaw delta angle - rad^2 const ftype dazVar = sq(EKFGSF_gyroNoise * angle_dt); // variance of yaw delta angle - rad^2
const float t2 = sinf(EKF[mdl_idx].X[2]); const ftype t2 = sinF(EKF[mdl_idx].X[2]);
const float t3 = cosf(EKF[mdl_idx].X[2]); const ftype t3 = cosF(EKF[mdl_idx].X[2]);
const float t4 = dvy*t3; const ftype t4 = dvy*t3;
const float t5 = dvx*t2; const ftype t5 = dvx*t2;
const float t6 = t4+t5; const ftype t6 = t4+t5;
const float t8 = P22*t6; const ftype t8 = P22*t6;
const float t7 = P02-t8; const ftype t7 = P02-t8;
const float t9 = dvx*t3; const ftype t9 = dvx*t3;
const float t11 = dvy*t2; const ftype t11 = dvy*t2;
const float t10 = t9-t11; const ftype t10 = t9-t11;
const float t12 = dvxVar*t2*t3; const ftype t12 = dvxVar*t2*t3;
const float t13 = t2*t2; const ftype t13 = t2*t2;
const float t14 = t3*t3; const ftype t14 = t3*t3;
const float t15 = P22*t10; const ftype t15 = P22*t10;
const float t16 = P12+t15; const ftype t16 = P12+t15;
const float min_var = 1e-6f; const ftype min_var = 1e-6f;
EKF[mdl_idx].P[0][0] = fmaxf(P00-P20*t6+dvxVar*t14+dvyVar*t13-t6*t7, min_var); EKF[mdl_idx].P[0][0] = fmaxF(P00-P20*t6+dvxVar*t14+dvyVar*t13-t6*t7, min_var);
EKF[mdl_idx].P[0][1] = P01+t12-P21*t6+t7*t10-dvyVar*t2*t3; EKF[mdl_idx].P[0][1] = P01+t12-P21*t6+t7*t10-dvyVar*t2*t3;
EKF[mdl_idx].P[0][2] = t7; EKF[mdl_idx].P[0][2] = t7;
EKF[mdl_idx].P[1][0] = P10+t12+P20*t10-t6*t16-dvyVar*t2*t3; EKF[mdl_idx].P[1][0] = P10+t12+P20*t10-t6*t16-dvyVar*t2*t3;
EKF[mdl_idx].P[1][1] = fmaxf(P11+P21*t10+dvxVar*t13+dvyVar*t14+t10*t16, min_var); EKF[mdl_idx].P[1][1] = fmaxF(P11+P21*t10+dvxVar*t13+dvyVar*t14+t10*t16, min_var);
EKF[mdl_idx].P[1][2] = t16; EKF[mdl_idx].P[1][2] = t16;
EKF[mdl_idx].P[2][0] = P20-t8; EKF[mdl_idx].P[2][0] = P20-t8;
EKF[mdl_idx].P[2][1] = P21+t15; EKF[mdl_idx].P[2][1] = P21+t15;
EKF[mdl_idx].P[2][2] = fmaxf(P22+dazVar, min_var); EKF[mdl_idx].P[2][2] = fmaxF(P22+dazVar, min_var);
// force symmetry // force symmetry
forceSymmetry(mdl_idx); forceSymmetry(mdl_idx);
@ -388,22 +388,22 @@ void EKFGSF_yaw::predict(const uint8_t mdl_idx)
// Update EKF states and covariance for specified model index using velocity measurement // Update EKF states and covariance for specified model index using velocity measurement
// Returns false if the sttae and covariance correction failed // Returns false if the sttae and covariance correction failed
bool EKFGSF_yaw::correct(const uint8_t mdl_idx, const Vector2f &vel, const float velObsVar) bool EKFGSF_yaw::correct(const uint8_t mdl_idx, const Vector2F &vel, const ftype velObsVar)
{ {
// calculate velocity observation innovations // calculate velocity observation innovations
EKF[mdl_idx].innov[0] = EKF[mdl_idx].X[0] - vel[0]; EKF[mdl_idx].innov[0] = EKF[mdl_idx].X[0] - vel[0];
EKF[mdl_idx].innov[1] = EKF[mdl_idx].X[1] - vel[1]; EKF[mdl_idx].innov[1] = EKF[mdl_idx].X[1] - vel[1];
// copy covariance matrix to temporary variables // copy covariance matrix to temporary variables
const float P00 = EKF[mdl_idx].P[0][0]; const ftype P00 = EKF[mdl_idx].P[0][0];
const float P01 = EKF[mdl_idx].P[0][1]; const ftype P01 = EKF[mdl_idx].P[0][1];
const float P02 = EKF[mdl_idx].P[0][2]; const ftype P02 = EKF[mdl_idx].P[0][2];
const float P10 = EKF[mdl_idx].P[1][0]; const ftype P10 = EKF[mdl_idx].P[1][0];
const float P11 = EKF[mdl_idx].P[1][1]; const ftype P11 = EKF[mdl_idx].P[1][1];
const float P12 = EKF[mdl_idx].P[1][2]; const ftype P12 = EKF[mdl_idx].P[1][2];
const float P20 = EKF[mdl_idx].P[2][0]; const ftype P20 = EKF[mdl_idx].P[2][0];
const float P21 = EKF[mdl_idx].P[2][1]; const ftype P21 = EKF[mdl_idx].P[2][1];
const float P22 = EKF[mdl_idx].P[2][2]; const ftype P22 = EKF[mdl_idx].P[2][2];
// calculate innovation variance // calculate innovation variance
EKF[mdl_idx].S[0][0] = P00 + velObsVar; EKF[mdl_idx].S[0][0] = P00 + velObsVar;
@ -412,22 +412,22 @@ bool EKFGSF_yaw::correct(const uint8_t mdl_idx, const Vector2f &vel, const float
EKF[mdl_idx].S[1][0] = P10; EKF[mdl_idx].S[1][0] = P10;
// Perform a chi-square innovation consistency test and calculate a compression scale factor that limits the magnitude of innovations to 5-sigma // Perform a chi-square innovation consistency test and calculate a compression scale factor that limits the magnitude of innovations to 5-sigma
float S_det_inv = (EKF[mdl_idx].S[0][0]*EKF[mdl_idx].S[1][1] - EKF[mdl_idx].S[0][1]*EKF[mdl_idx].S[1][0]); ftype S_det_inv = (EKF[mdl_idx].S[0][0]*EKF[mdl_idx].S[1][1] - EKF[mdl_idx].S[0][1]*EKF[mdl_idx].S[1][0]);
float innov_comp_scale_factor = 1.0f; ftype innov_comp_scale_factor = 1.0f;
if (fabsf(S_det_inv) > 1E-6f) { if (fabsF(S_det_inv) > 1E-6f) {
// Calculate elements for innovation covariance inverse matrix assuming symmetry // Calculate elements for innovation covariance inverse matrix assuming symmetry
S_det_inv = 1.0f / S_det_inv; S_det_inv = 1.0f / S_det_inv;
const float S_inv_NN = EKF[mdl_idx].S[1][1] * S_det_inv; const ftype S_inv_NN = EKF[mdl_idx].S[1][1] * S_det_inv;
const float S_inv_EE = EKF[mdl_idx].S[0][0] * S_det_inv; const ftype S_inv_EE = EKF[mdl_idx].S[0][0] * S_det_inv;
const float S_inv_NE = EKF[mdl_idx].S[0][1] * S_det_inv; const ftype S_inv_NE = EKF[mdl_idx].S[0][1] * S_det_inv;
// The following expression was derived symbolically from test ratio = transpose(innovation) * inverse(innovation variance) * innovation = [1x2] * [2,2] * [2,1] = [1,1] // The following expression was derived symbolically from test ratio = transpose(innovation) * inverse(innovation variance) * innovation = [1x2] * [2,2] * [2,1] = [1,1]
const float test_ratio = EKF[mdl_idx].innov[0]*(EKF[mdl_idx].innov[0]*S_inv_NN + EKF[mdl_idx].innov[1]*S_inv_NE) + EKF[mdl_idx].innov[1]*(EKF[mdl_idx].innov[0]*S_inv_NE + EKF[mdl_idx].innov[1]*S_inv_EE); const ftype test_ratio = EKF[mdl_idx].innov[0]*(EKF[mdl_idx].innov[0]*S_inv_NN + EKF[mdl_idx].innov[1]*S_inv_NE) + EKF[mdl_idx].innov[1]*(EKF[mdl_idx].innov[0]*S_inv_NE + EKF[mdl_idx].innov[1]*S_inv_EE);
// If the test ratio is greater than 25 (5 Sigma) then reduce the length of the innovation vector to clip it at 5-Sigma // 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 // This protects from large measurement spikes
if (test_ratio > 25.0f) { if (test_ratio > 25.0f) {
innov_comp_scale_factor = sqrtf(25.0f / test_ratio); innov_comp_scale_factor = sqrtF(25.0f / test_ratio);
} }
} else { } else {
// skip this fusion step because calculation is badly conditioned // skip this fusion step because calculation is badly conditioned
@ -437,22 +437,22 @@ bool EKFGSF_yaw::correct(const uint8_t mdl_idx, const Vector2f &vel, const float
// calculate Kalman gain K and covariance matrix P // calculate Kalman gain K and covariance matrix P
// autocode from https://github.com/priseborough/3_state_filter/blob/flightLogReplay-wip/calcK.txt // 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 // and https://github.com/priseborough/3_state_filter/blob/flightLogReplay-wip/calcPmat.txt
const float t2 = P00*velObsVar; const ftype t2 = P00*velObsVar;
const float t3 = P11*velObsVar; const ftype t3 = P11*velObsVar;
const float t4 = velObsVar*velObsVar; const ftype t4 = velObsVar*velObsVar;
const float t5 = P00*P11; const ftype t5 = P00*P11;
const float t9 = P01*P10; const ftype t9 = P01*P10;
const float t6 = t2+t3+t4+t5-t9; const ftype t6 = t2+t3+t4+t5-t9;
float t7; ftype t7;
if (fabsf(t6) > 1e-6f) { if (fabsF(t6) > 1e-6f) {
t7 = 1.0f/t6; t7 = 1.0f/t6;
} else { } else {
// skip this fusion step // skip this fusion step
return false; return false;
} }
const float t8 = P11+velObsVar; const ftype t8 = P11+velObsVar;
const float t10 = P00+velObsVar; const ftype t10 = P00+velObsVar;
float K[3][2]; ftype K[3][2];
K[0][0] = -P01*P10*t7+P00*t7*t8; K[0][0] = -P01*P10*t7+P00*t7*t8;
K[0][1] = -P00*P01*t7+P01*t7*t10; K[0][1] = -P00*P01*t7+P01*t7*t10;
@ -461,70 +461,70 @@ bool EKFGSF_yaw::correct(const uint8_t mdl_idx, const Vector2f &vel, const float
K[2][0] = -P10*P21*t7+P20*t7*t8; K[2][0] = -P10*P21*t7+P20*t7*t8;
K[2][1] = -P01*P20*t7+P21*t7*t10; K[2][1] = -P01*P20*t7+P21*t7*t10;
const float t11 = P00*P01*t7; const ftype t11 = P00*P01*t7;
const float t15 = P01*t7*t10; const ftype t15 = P01*t7*t10;
const float t12 = t11-t15; const ftype t12 = t11-t15;
const float t13 = P01*P10*t7; const ftype t13 = P01*P10*t7;
const float t16 = P00*t7*t8; const ftype t16 = P00*t7*t8;
const float t14 = t13-t16; const ftype t14 = t13-t16;
const float t17 = t8*t12; const ftype t17 = t8*t12;
const float t18 = P01*t14; const ftype t18 = P01*t14;
const float t19 = t17+t18; const ftype t19 = t17+t18;
const float t20 = t10*t14; const ftype t20 = t10*t14;
const float t21 = P10*t12; const ftype t21 = P10*t12;
const float t22 = t20+t21; const ftype t22 = t20+t21;
const float t27 = P11*t7*t10; const ftype t27 = P11*t7*t10;
const float t23 = t13-t27; const ftype t23 = t13-t27;
const float t24 = P10*P11*t7; const ftype t24 = P10*P11*t7;
const float t26 = P10*t7*t8; const ftype t26 = P10*t7*t8;
const float t25 = t24-t26; const ftype t25 = t24-t26;
const float t28 = t8*t23; const ftype t28 = t8*t23;
const float t29 = P01*t25; const ftype t29 = P01*t25;
const float t30 = t28+t29; const ftype t30 = t28+t29;
const float t31 = t10*t25; const ftype t31 = t10*t25;
const float t32 = P10*t23; const ftype t32 = P10*t23;
const float t33 = t31+t32; const ftype t33 = t31+t32;
const float t34 = P01*P20*t7; const ftype t34 = P01*P20*t7;
const float t38 = P21*t7*t10; const ftype t38 = P21*t7*t10;
const float t35 = t34-t38; const ftype t35 = t34-t38;
const float t36 = P10*P21*t7; const ftype t36 = P10*P21*t7;
const float t39 = P20*t7*t8; const ftype t39 = P20*t7*t8;
const float t37 = t36-t39; const ftype t37 = t36-t39;
const float t40 = t8*t35; const ftype t40 = t8*t35;
const float t41 = P01*t37; const ftype t41 = P01*t37;
const float t42 = t40+t41; const ftype t42 = t40+t41;
const float t43 = t10*t37; const ftype t43 = t10*t37;
const float t44 = P10*t35; const ftype t44 = P10*t35;
const float t45 = t43+t44; const ftype t45 = t43+t44;
const float min_var = 1e-6f; const ftype min_var = 1e-6f;
EKF[mdl_idx].P[0][0] = fmaxf(P00-t12*t19-t14*t22, min_var); EKF[mdl_idx].P[0][0] = fmaxF(P00-t12*t19-t14*t22, min_var);
EKF[mdl_idx].P[0][1] = P01-t19*t23-t22*t25; EKF[mdl_idx].P[0][1] = P01-t19*t23-t22*t25;
EKF[mdl_idx].P[0][2] = P02-t19*t35-t22*t37; EKF[mdl_idx].P[0][2] = P02-t19*t35-t22*t37;
EKF[mdl_idx].P[1][0] = P10-t12*t30-t14*t33; EKF[mdl_idx].P[1][0] = P10-t12*t30-t14*t33;
EKF[mdl_idx].P[1][1] = fmaxf(P11-t23*t30-t25*t33, min_var); EKF[mdl_idx].P[1][1] = fmaxF(P11-t23*t30-t25*t33, min_var);
EKF[mdl_idx].P[1][2] = P12-t30*t35-t33*t37; EKF[mdl_idx].P[1][2] = P12-t30*t35-t33*t37;
EKF[mdl_idx].P[2][0] = P20-t12*t42-t14*t45; EKF[mdl_idx].P[2][0] = P20-t12*t42-t14*t45;
EKF[mdl_idx].P[2][1] = P21-t23*t42-t25*t45; EKF[mdl_idx].P[2][1] = P21-t23*t42-t25*t45;
EKF[mdl_idx].P[2][2] = fmaxf(P22-t35*t42-t37*t45, min_var); EKF[mdl_idx].P[2][2] = fmaxF(P22-t35*t42-t37*t45, min_var);
// force symmetry // force symmetry
forceSymmetry(mdl_idx); forceSymmetry(mdl_idx);
// Apply state corrections and capture change in yaw angle // Apply state corrections and capture change in yaw angle
const float yaw_prev = EKF[mdl_idx].X[2]; const ftype yaw_prev = EKF[mdl_idx].X[2];
for (uint8_t obs_index = 0; obs_index < 2; obs_index++) { for (uint8_t obs_index = 0; obs_index < 2; obs_index++) {
// apply the state corrections including the compression scale factor // apply the state corrections including the compression scale factor
for (unsigned row = 0; row < 3; row++) { for (unsigned row = 0; row < 3; row++) {
EKF[mdl_idx].X[row] -= K[row][obs_index] * EKF[mdl_idx].innov[obs_index] * innov_comp_scale_factor; EKF[mdl_idx].X[row] -= K[row][obs_index] * EKF[mdl_idx].innov[obs_index] * innov_comp_scale_factor;
} }
} }
const float yaw_delta = EKF[mdl_idx].X[2] - yaw_prev; const ftype yaw_delta = EKF[mdl_idx].X[2] - yaw_prev;
// apply the change in yaw angle to the AHRS taking advantage of sparseness in the yaw rotation matrix // apply the change in yaw angle to the AHRS taking advantage of sparseness in the yaw rotation matrix
const float cos_yaw = cosf(yaw_delta); const ftype cos_yaw = cosF(yaw_delta);
const float sin_yaw = sinf(yaw_delta); const ftype sin_yaw = sinF(yaw_delta);
float R_prev[2][3]; ftype R_prev[2][3];
memcpy(&R_prev, &AHRS[mdl_idx].R, sizeof(R_prev)); // copy first two rows from 3x3 memcpy(&R_prev, &AHRS[mdl_idx].R, sizeof(R_prev)); // copy first two rows from 3x3
AHRS[mdl_idx].R[0][0] = R_prev[0][0] * cos_yaw - R_prev[1][0] * sin_yaw; AHRS[mdl_idx].R[0][0] = R_prev[0][0] * cos_yaw - R_prev[1][0] * sin_yaw;
AHRS[mdl_idx].R[0][1] = R_prev[0][1] * cos_yaw - R_prev[1][1] * sin_yaw; AHRS[mdl_idx].R[0][1] = R_prev[0][1] * cos_yaw - R_prev[1][1] * sin_yaw;
@ -543,7 +543,7 @@ void EKFGSF_yaw::resetEKFGSF()
run_ekf_gsf = false; run_ekf_gsf = false;
memset(&EKF, 0, sizeof(EKF)); memset(&EKF, 0, sizeof(EKF));
const float yaw_increment = M_2PI / (float)N_MODELS_EKFGSF; const ftype yaw_increment = M_2PI / (float)N_MODELS_EKFGSF;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) { for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
// evenly space initial yaw estimates in the region between +-Pi // evenly space initial yaw estimates in the region between +-Pi
EKF[mdl_idx].X[2] = -M_PI + (0.5f * yaw_increment) + ((float)mdl_idx * yaw_increment); EKF[mdl_idx].X[2] = -M_PI + (0.5f * yaw_increment) + ((float)mdl_idx * yaw_increment);
@ -558,48 +558,48 @@ void EKFGSF_yaw::resetEKFGSF()
} }
// returns the probability of a selected model output assuming a gaussian error distribution // returns the probability of a selected model output assuming a gaussian error distribution
float EKFGSF_yaw::gaussianDensity(const uint8_t mdl_idx) const ftype EKFGSF_yaw::gaussianDensity(const uint8_t mdl_idx) const
{ {
const float t2 = EKF[mdl_idx].S[0][0] * EKF[mdl_idx].S[1][1]; const ftype t2 = EKF[mdl_idx].S[0][0] * EKF[mdl_idx].S[1][1];
const float t5 = EKF[mdl_idx].S[0][1] * EKF[mdl_idx].S[1][0]; const ftype t5 = EKF[mdl_idx].S[0][1] * EKF[mdl_idx].S[1][0];
const float t3 = t2 - t5; // determinant const ftype t3 = t2 - t5; // determinant
const float t4 = 1.0f / MAX(t3, 1e-12f); // determinant inverse const ftype t4 = 1.0f / MAX(t3, 1e-12f); // determinant inverse
// inv(S) // inv(S)
float invMat[2][2]; ftype invMat[2][2];
invMat[0][0] = t4 * EKF[mdl_idx].S[1][1]; invMat[0][0] = t4 * EKF[mdl_idx].S[1][1];
invMat[1][1] = t4 * EKF[mdl_idx].S[0][0]; invMat[1][1] = t4 * EKF[mdl_idx].S[0][0];
invMat[0][1] = - t4 * EKF[mdl_idx].S[0][1]; invMat[0][1] = - t4 * EKF[mdl_idx].S[0][1];
invMat[1][0] = - t4 * EKF[mdl_idx].S[1][0]; invMat[1][0] = - t4 * EKF[mdl_idx].S[1][0];
// inv(S) * innovation // inv(S) * innovation
float tempVec[2]; ftype tempVec[2];
tempVec[0] = invMat[0][0] * EKF[mdl_idx].innov[0] + invMat[0][1] * EKF[mdl_idx].innov[1]; tempVec[0] = invMat[0][0] * EKF[mdl_idx].innov[0] + invMat[0][1] * EKF[mdl_idx].innov[1];
tempVec[1] = invMat[1][0] * EKF[mdl_idx].innov[0] + invMat[1][1] * EKF[mdl_idx].innov[1]; tempVec[1] = invMat[1][0] * EKF[mdl_idx].innov[0] + invMat[1][1] * EKF[mdl_idx].innov[1];
// transpose(innovation) * inv(S) * innovation // transpose(innovation) * inv(S) * innovation
float normDist = tempVec[0] * EKF[mdl_idx].innov[0] + tempVec[1] * EKF[mdl_idx].innov[1]; ftype normDist = tempVec[0] * EKF[mdl_idx].innov[0] + tempVec[1] * EKF[mdl_idx].innov[1];
// convert from a normalised variance to a probability assuming a Gaussian distribution // convert from a normalised variance to a probability assuming a Gaussian distribution
normDist = expf(-0.5f * normDist); normDist = expf(-0.5f * normDist);
normDist *= sqrtf(t4)/ M_2PI; normDist *= sqrtF(t4)/ M_2PI;
return normDist; return normDist;
} }
void EKFGSF_yaw::forceSymmetry(const uint8_t mdl_idx) void EKFGSF_yaw::forceSymmetry(const uint8_t mdl_idx)
{ {
float P01 = 0.5f * (EKF[mdl_idx].P[0][1] + EKF[mdl_idx].P[1][0]); ftype P01 = 0.5f * (EKF[mdl_idx].P[0][1] + EKF[mdl_idx].P[1][0]);
float P02 = 0.5f * (EKF[mdl_idx].P[0][2] + EKF[mdl_idx].P[2][0]); ftype P02 = 0.5f * (EKF[mdl_idx].P[0][2] + EKF[mdl_idx].P[2][0]);
float P12 = 0.5f * (EKF[mdl_idx].P[1][2] + EKF[mdl_idx].P[2][1]); ftype P12 = 0.5f * (EKF[mdl_idx].P[1][2] + EKF[mdl_idx].P[2][1]);
EKF[mdl_idx].P[0][1] = EKF[mdl_idx].P[1][0] = P01; EKF[mdl_idx].P[0][1] = EKF[mdl_idx].P[1][0] = P01;
EKF[mdl_idx].P[0][2] = EKF[mdl_idx].P[2][0] = P02; EKF[mdl_idx].P[0][2] = EKF[mdl_idx].P[2][0] = P02;
EKF[mdl_idx].P[1][2] = EKF[mdl_idx].P[2][1] = P12; EKF[mdl_idx].P[1][2] = EKF[mdl_idx].P[2][1] = P12;
} }
// Apply a body frame delta angle to the body to earth frame rotation matrix using a small angle approximation // Apply a body frame delta angle to the body to earth frame rotation matrix using a small angle approximation
Matrix3f EKFGSF_yaw::updateRotMat(const Matrix3f &R, const Vector3f &g) const Matrix3F EKFGSF_yaw::updateRotMat(const Matrix3F &R, const Vector3F &g) const
{ {
Matrix3f ret = R; Matrix3F ret = R;
ret[0][0] += R[0][1] * g[2] - R[0][2] * g[1]; 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][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[0][2] += R[0][0] * g[1] - R[0][1] * g[0];
@ -611,13 +611,13 @@ Matrix3f EKFGSF_yaw::updateRotMat(const Matrix3f &R, const Vector3f &g) const
ret[2][2] += R[2][0] * g[1] - R[2][1] * g[0]; ret[2][2] += R[2][0] * g[1] - R[2][1] * g[0];
// Renormalise rows // Renormalise rows
float rowLengthSq; ftype rowLengthSq;
for (uint8_t r = 0; r < 3; r++) { for (uint8_t r = 0; r < 3; r++) {
rowLengthSq = ret[r][0] * ret[r][0] + ret[r][1] * ret[r][1] + ret[r][2] * ret[r][2]; rowLengthSq = ret[r][0] * ret[r][0] + ret[r][1] * ret[r][1] + ret[r][2] * ret[r][2];
if (is_positive(rowLengthSq)) { if (is_positive(rowLengthSq)) {
// Use linear approximation for inverse sqrt taking advantage of the row length being close to 1.0 // 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; const ftype rowLengthInv = 1.5f - 0.5f * rowLengthSq;
Vector3f &row = ret[r]; Vector3F &row = ret[r];
row *= rowLengthInv; row *= rowLengthInv;
} }
} }
@ -625,7 +625,7 @@ Matrix3f EKFGSF_yaw::updateRotMat(const Matrix3f &R, const Vector3f &g) const
return ret; return ret;
} }
bool EKFGSF_yaw::getYawData(float &yaw, float &yawVariance) const bool EKFGSF_yaw::getYawData(ftype &yaw, ftype &yawVariance) const
{ {
if (!vel_fuse_running) { if (!vel_fuse_running) {
return false; return false;
@ -635,14 +635,14 @@ bool EKFGSF_yaw::getYawData(float &yaw, float &yawVariance) const
return true; return true;
} }
bool EKFGSF_yaw::getVelInnovLength(float &velInnovLength) const bool EKFGSF_yaw::getVelInnovLength(ftype &velInnovLength) const
{ {
if (!vel_fuse_running) { if (!vel_fuse_running) {
return false; return false;
} }
velInnovLength = 0.0f; velInnovLength = 0.0f;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) { for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
velInnovLength += GSF.weights[mdl_idx] * sqrtf((sq(EKF[mdl_idx].innov[0]) + sq(EKF[mdl_idx].innov[1]))); velInnovLength += GSF.weights[mdl_idx] * sqrtF((sq(EKF[mdl_idx].innov[0]) + sq(EKF[mdl_idx].innov[1])));
} }
return true; return true;
} }
@ -650,6 +650,6 @@ bool EKFGSF_yaw::getVelInnovLength(float &velInnovLength) const
void EKFGSF_yaw::setGyroBias(Vector3f &gyroBias) void EKFGSF_yaw::setGyroBias(Vector3f &gyroBias)
{ {
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) { for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
AHRS[mdl_idx].gyro_bias = gyroBias; AHRS[mdl_idx].gyro_bias = gyroBias.toftype();
} }
} }

77
libraries/AP_NavEKF/EKFGSF_yaw.h
View File

@ -16,28 +16,28 @@ public:
EKFGSF_yaw(); EKFGSF_yaw();
// Update Filter States - this should be called whenever new IMU data is available // Update Filter States - this should be called whenever new IMU data is available
void update(const Vector3f &delAng,// IMU delta angle rotation vector meassured in body frame (rad) void update(const Vector3F &delAng,// IMU delta angle rotation vector meassured in body frame (rad)
const Vector3f &delVel,// IMU delta velocity vector meassured in body frame (m/s) const Vector3F &delVel,// IMU delta velocity vector meassured in body frame (m/s)
const float delAngDT, // time interval that delAng was integrated over (sec) - must be no less than IMU_DT_MIN_SEC const ftype delAngDT, // time interval that delAng was integrated over (sec) - must be no less than IMU_DT_MIN_SEC
const float delVelDT, // time interval that delVel was integrated over (sec) - must be no less than IMU_DT_MIN_SEC const ftype delVelDT, // time interval that delVel was integrated over (sec) - must be no less than IMU_DT_MIN_SEC
bool runEKF, // set to true when flying or movement suitable for yaw estimation bool runEKF, // set to true when flying or movement suitable for yaw estimation
float TAS); // true airspeed used for centripetal accel compensation - set to 0 when not required. ftype TAS); // true airspeed used for centripetal accel compensation - set to 0 when not required.
// Fuse NE velocty mesurements and update the EKF's and GSF state and covariance estimates // Fuse NE velocty mesurements and update the EKF's and GSF state and covariance estimates
// Should be called after update(...) whenever new velocity data is available // Should be called after update(...) whenever new velocity data is available
void fuseVelData(const Vector2f &vel, // NE velocity measurement (m/s) void fuseVelData(const Vector2F &vel, // NE velocity measurement (m/s)
const float velAcc); // 1-sigma accuracy of velocity measurement (m/s) const ftype velAcc); // 1-sigma accuracy of velocity measurement (m/s)
// set the gyro bias in rad/sec // set the gyro bias in rad/sec
void setGyroBias(Vector3f &gyroBias); void setGyroBias(Vector3f &gyroBias);
// get yaw estimated and corresponding variance // get yaw estimated and corresponding variance
// return false if yaw estimation is inactive // return false if yaw estimation is inactive
bool getYawData(float &yaw, float &yawVariance) const; bool getYawData(ftype &yaw, ftype &yawVariance) const;
// get the length of the weighted average velocity innovation vector // get the length of the weighted average velocity innovation vector
// return false if not available // return false if not available
bool getVelInnovLength(float &velInnovLength) const; bool getVelInnovLength(ftype &velInnovLength) const;
// log EKFGSF data on behalf of an EKF caller. id0 and id1 are the // log EKFGSF data on behalf of an EKF caller. id0 and id1 are the
// IDs of the messages to log, e.g. LOG_NKY0_MSG, LOG_NKY1_MSG // IDs of the messages to log, e.g. LOG_NKY0_MSG, LOG_NKY1_MSG
@ -45,7 +45,6 @@ public:
private: private:
typedef float ftype;
#if MATH_CHECK_INDEXES #if MATH_CHECK_INDEXES
typedef VectorN<ftype,2> Vector2; typedef VectorN<ftype,2> Vector2;
typedef VectorN<ftype,3> Vector3; typedef VectorN<ftype,3> Vector3;
@ -58,40 +57,40 @@ private:
#endif #endif
// Parameters // Parameters
const float EKFGSF_gyroNoise{1.0e-1f}; // yaw rate noise used for covariance prediction (rad/sec) const ftype EKFGSF_gyroNoise{1.0e-1}; // yaw rate noise used for covariance prediction (rad/sec)
const float EKFGSF_accelNoise{2.0f}; // horizontal accel noise used for covariance prediction (m/sec**2) const ftype EKFGSF_accelNoise{2.0}; // horizontal accel noise used for covariance prediction (m/sec**2)
const float EKFGSF_tiltGain{0.2f}; // gain from tilt error to gyro correction for complementary filter (1/sec) const ftype EKFGSF_tiltGain{0.2}; // gain from tilt error to gyro correction for complementary filter (1/sec)
const float EKFGSF_gyroBiasGain{0.04f}; // gain applied to integral of gyro correction for complementary filter (1/sec) const ftype EKFGSF_gyroBiasGain{0.04}; // gain applied to integral of gyro correction for complementary filter (1/sec)
const float EKFGSF_accelFiltRatio{10.0f}; // ratio of time constant of AHRS tilt correction to time constant of first order LPF applied to accel data used by ahrs const ftype EKFGSF_accelFiltRatio{10.0}; // ratio of time constant of AHRS tilt correction to time constant of first order LPF applied to accel data used by ahrs
// Declarations used by the bank of AHRS complementary filters that use IMU data augmented by true // Declarations used by the bank of AHRS complementary filters that use IMU data augmented by true
// airspeed data when in fixed wing mode to estimate the quaternions that are used to rotate IMU data into a // airspeed data when in fixed wing mode to estimate the quaternions that are used to rotate IMU data into a
// Front, Right, Yaw frame of reference. // Front, Right, Yaw frame of reference.
Vector3f delta_angle; Vector3F delta_angle;
Vector3f delta_velocity; Vector3F delta_velocity;
float angle_dt; ftype angle_dt;
float velocity_dt; ftype velocity_dt;
struct ahrs_struct { struct ahrs_struct {
Matrix3f R; // matrix that rotates a vector from body to earth frame Matrix3F R; // matrix that rotates a vector from body to earth frame
Vector3f gyro_bias; // gyro bias learned and used by the quaternion calculation Vector3F gyro_bias; // gyro bias learned and used by the quaternion calculation
bool aligned{false}; // true when AHRS has been aligned bool aligned; // true when AHRS has been aligned
float accel_FR[2]; // front-right acceleration vector in a horizontal plane (m/s/s) ftype accel_FR[2]; // front-right acceleration vector in a horizontal plane (m/s/s)
float vel_NE[2]; // NE velocity vector from last GPS measurement (m/s) ftype vel_NE[2]; // NE velocity vector from last GPS measurement (m/s)
bool fuse_gps; // true when GPS should be fused on that frame bool fuse_gps; // true when GPS should be fused on that frame
float accel_dt; // time step used when generating _simple_accel_FR data (sec) ftype accel_dt; // time step used when generating _simple_accel_FR data (sec)
}; };
ahrs_struct AHRS[N_MODELS_EKFGSF]; ahrs_struct AHRS[N_MODELS_EKFGSF];
bool ahrs_tilt_aligned; // true the initial tilt alignment has been calculated bool ahrs_tilt_aligned; // true the initial tilt alignment has been calculated
float accel_gain; // gain from accel vector tilt error to rate gyro correction used by AHRS calculation ftype accel_gain; // gain from accel vector tilt error to rate gyro correction used by AHRS calculation
Vector3f ahrs_accel; // filtered body frame specific force vector used by AHRS calculation (m/s/s) Vector3F ahrs_accel; // filtered body frame specific force vector used by AHRS calculation (m/s/s)
float ahrs_accel_norm; // length of body frame specific force vector used by AHRS calculation (m/s/s) ftype ahrs_accel_norm; // length of body frame specific force vector used by AHRS calculation (m/s/s)
float true_airspeed; // true airspeed used to correct for centripetal acceleratoin in coordinated turns (m/s) ftype true_airspeed; // true airspeed used to correct for centripetal acceleratoin in coordinated turns (m/s)
// Runs quaternion prediction for the selected AHRS using IMU (and optionally true airspeed) data // Runs quaternion prediction for the selected AHRS using IMU (and optionally true airspeed) data
void predictAHRS(const uint8_t mdl_idx); void predictAHRS(const uint8_t mdl_idx);
// Applies a body frame delta angle to a body to earth frame rotation matrix using a small angle approximation // Applies a body frame delta angle to a body to earth frame rotation matrix using a small angle approximation
Matrix3f updateRotMat(const Matrix3f &R, const Vector3f &g) const; Matrix3F updateRotMat(const Matrix3F &R, const Vector3F &g) const;
// Initialises the tilt (roll and pitch) for all AHRS using IMU acceleration data // Initialises the tilt (roll and pitch) for all AHRS using IMU acceleration data
void alignTilt(); void alignTilt();
@ -102,10 +101,10 @@ private:
// The Following declarations are used by bank of EKF's that estimate yaw angle starting from a different yaw hypothesis for each filter. // The Following declarations are used by bank of EKF's that estimate yaw angle starting from a different yaw hypothesis for each filter.
struct EKF_struct { struct EKF_struct {
float X[3]; // Vel North (m/s), Vel East (m/s), yaw (rad) ftype X[3]; // Vel North (m/s), Vel East (m/s), yaw (rad)
float P[3][3]; // covariance matrix ftype P[3][3]; // covariance matrix
float S[2][2]; // N,E velocity innovation variance (m/s)^2 ftype S[2][2]; // N,E velocity innovation variance (m/s)^2
float innov[2]; // Velocity N,E innovation (m/s) ftype innov[2]; // Velocity N,E innovation (m/s)
}; };
EKF_struct EKF[N_MODELS_EKFGSF]; EKF_struct EKF[N_MODELS_EKFGSF];
bool vel_fuse_running; // true when the bank of EKF's has started fusing GPS velocity data bool vel_fuse_running; // true when the bank of EKF's has started fusing GPS velocity data
@ -119,7 +118,7 @@ private:
// Runs the state and covariance update for the selected EKF using the GPS NE velocity measurement // Runs the state and covariance update for the selected EKF using the GPS NE velocity measurement
// Returns false if the sttae and covariance correction failed // Returns false if the sttae and covariance correction failed
bool correct(const uint8_t mdl_idx, const Vector2f &vel, const float velObsVar); bool correct(const uint8_t mdl_idx, const Vector2F &vel, const ftype velObsVar);
// Forces symmetry on the covariance matrix for the selected EKF // Forces symmetry on the covariance matrix for the selected EKF
void forceSymmetry(const uint8_t mdl_idx); void forceSymmetry(const uint8_t mdl_idx);
@ -128,13 +127,13 @@ private:
// EKF's to form a single state estimate. // EKF's to form a single state estimate.
struct GSF_struct { struct GSF_struct {
float yaw; // yaw (rad) ftype yaw; // yaw (rad)
float yaw_variance; // Yaw state variance (rad^2) ftype yaw_variance; // Yaw state variance (rad^2)
float weights[N_MODELS_EKFGSF]; // Weighting applied to each EKF model. Sum of weights is unity. ftype weights[N_MODELS_EKFGSF]; // Weighting applied to each EKF model. Sum of weights is unity.
}; };
GSF_struct GSF; GSF_struct GSF;
// Returns the probability for a selected model assuming a Gaussian error distribution // Returns the probability for a selected model assuming a Gaussian error distribution
// Used by the Guassian Sum Filter to calculate the weightings when combining the outputs from the bank of EKF's // Used by the Guassian Sum Filter to calculate the weightings when combining the outputs from the bank of EKF's
float gaussianDensity(const uint8_t mdl_idx) const; ftype gaussianDensity(const uint8_t mdl_idx) const;
}; };