mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF: allow for double EKF build
This commit is contained in:
Andrew Tridgell
parent
32a83ef347
commit
3235747ef8
|
|
@ -31,9 +31,9 @@ void NavEKF_core_common::fill_scratch_variables(void)
|
|||
// SITL where they are used without initialisation. These are all
|
||||
// supposed to be scratch variables that are not used between
|
||||
// iterations
|
||||
fill_nanf(&KH[0][0], sizeof(KH)/sizeof(float));
|
||||
fill_nanf(&KHP[0][0], sizeof(KHP)/sizeof(float));
|
||||
fill_nanf(&nextP[0][0], sizeof(nextP)/sizeof(float));
|
||||
fill_nanf(&Kfusion[0], sizeof(Kfusion)/sizeof(float));
|
||||
fill_nanf(&KH[0][0], sizeof(KH)/sizeof(ftype));
|
||||
fill_nanf(&KHP[0][0], sizeof(KHP)/sizeof(ftype));
|
||||
fill_nanf(&nextP[0][0], sizeof(nextP)/sizeof(ftype));
|
||||
fill_nanf(&Kfusion[0], sizeof(Kfusion)/sizeof(ftype));
|
||||
#endif
|
||||
}
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@
|
|||
#include <stdint.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_Math/vectorN.h>
|
||||
#include "AP_Nav_Common.h"
|
||||
|
||||
/*
|
||||
this declares a common parent class for AP_NavEKF2 and
|
||||
|
|
@ -32,7 +33,6 @@
|
|||
*/
|
||||
class NavEKF_core_common {
|
||||
public:
|
||||
typedef float ftype;
|
||||
#if MATH_CHECK_INDEXES
|
||||
typedef VectorN<ftype,28> Vector28;
|
||||
typedef VectorN<VectorN<ftype,24>,24> Matrix24;
|
||||
|
|
@ -49,4 +49,15 @@ protected:
|
|||
|
||||
// fill all the common scratch variables with NaN on SITL
|
||||
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
|
||||
|
||||
|
|
|
|||
|
|
@ -17,6 +17,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
union nav_filter_status {
|
||||
struct {
|
||||
|
|
|
|||
|
|
@ -2,6 +2,8 @@
|
|||
|
||||
#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)
|
||||
{
|
||||
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,
|
||||
core : core_index,
|
||||
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],
|
||||
yaw1 : EKF[1].X[2],
|
||||
yaw2 : EKF[2].X[2],
|
||||
|
|
|
|||
|
|
@ -28,12 +28,12 @@
|
|||
|
||||
EKFGSF_yaw::EKFGSF_yaw() {};
|
||||
|
||||
void EKFGSF_yaw::update(const Vector3f &delAng,
|
||||
const Vector3f &delVel,
|
||||
const float delAngDT,
|
||||
const float delVelDT,
|
||||
void EKFGSF_yaw::update(const Vector3F &delAng,
|
||||
const Vector3F &delVel,
|
||||
const ftype delAngDT,
|
||||
const ftype delVelDT,
|
||||
bool runEKF,
|
||||
float TAS)
|
||||
ftype TAS)
|
||||
{
|
||||
|
||||
// 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
|
||||
// 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 Vector3f accel = delVel / fmaxf(delVelDT, 0.001f);
|
||||
const ftype filter_coef = fminF(EKFGSF_accelFiltRatio * delVelDT * EKFGSF_tiltGain, 1.0f);
|
||||
const Vector3F accel = delVel / fmaxF(delVelDT, 0.001f);
|
||||
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
|
||||
if (!ahrs_tilt_aligned) {
|
||||
const float accel_norm_sq = accel.length_squared();
|
||||
const float upper_accel_limit = GRAVITY_MSS * 1.1f;
|
||||
const float lower_accel_limit = GRAVITY_MSS * 0.9f;
|
||||
const ftype accel_norm_sq = accel.length_squared();
|
||||
const ftype upper_accel_limit = GRAVITY_MSS * 1.1f;
|
||||
const ftype lower_accel_limit = GRAVITY_MSS * 0.9f;
|
||||
const bool ok_to_align = ((accel_norm_sq > lower_accel_limit * lower_accel_limit &&
|
||||
accel_norm_sq < upper_accel_limit * upper_accel_limit));
|
||||
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
|
||||
// and zero at the min and max g limits. This reduces the effect of large g transients on the attitude
|
||||
// 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 (is_positive(true_airspeed)) {
|
||||
// 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.
|
||||
// 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 = {};
|
||||
Vector2F yaw_vector = {};
|
||||
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[1] += GSF.weights[mdl_idx] * sinf(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]);
|
||||
}
|
||||
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
|
||||
/*
|
||||
memset(&GSF.P, 0, sizeof(GSF.P));
|
||||
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++) {
|
||||
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;
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
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
|
||||
if (run_ekf_gsf) {
|
||||
|
|
@ -154,8 +154,8 @@ void EKFGSF_yaw::fuseVelData(const Vector2f &vel, const float velAcc)
|
|||
alignYaw();
|
||||
vel_fuse_running = true;
|
||||
} else {
|
||||
float total_w = 0.0f;
|
||||
float newWeight[(uint8_t)N_MODELS_EKFGSF];
|
||||
ftype total_w = 0.0f;
|
||||
ftype newWeight[(uint8_t)N_MODELS_EKFGSF];
|
||||
bool state_update_failed = false;
|
||||
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
|
||||
|
|
@ -166,7 +166,7 @@ void EKFGSF_yaw::fuseVelData(const Vector2f &vel, const float velAcc)
|
|||
|
||||
if (!state_update_failed) {
|
||||
// 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;
|
||||
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; 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
|
||||
// Reset the filters if all weights have underflowed due to excessive innovation variances
|
||||
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 ++) {
|
||||
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
|
||||
|
||||
// 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
|
||||
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).
|
||||
// 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) {
|
||||
|
||||
Vector3f accel = ahrs_accel;
|
||||
Vector3F accel = ahrs_accel;
|
||||
|
||||
if (is_positive(true_airspeed)) {
|
||||
// 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
|
||||
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
|
||||
accel -= centripetal_accel_vec_bf;
|
||||
|
|
@ -225,18 +225,18 @@ void EKFGSF_yaw::predictAHRS(const uint8_t mdl_idx)
|
|||
}
|
||||
|
||||
// Gyro bias estimation
|
||||
const float gyro_bias_limit = radians(5.0f);
|
||||
const float spinRate = ang_rate_delayed_raw.length();
|
||||
const ftype gyro_bias_limit = radians(5.0f);
|
||||
const ftype spinRate = ang_rate_delayed_raw.length();
|
||||
if (spinRate < 0.175f) {
|
||||
AHRS[mdl_idx].gyro_bias -= tilt_error_gyro_correction * (EKFGSF_gyroBiasGain * angle_dt);
|
||||
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
AHRS[mdl_idx].gyro_bias[i] = constrain_float(AHRS[mdl_idx].gyro_bias[i], -gyro_bias_limit, gyro_bias_limit);
|
||||
AHRS[mdl_idx].gyro_bias[i] = constrain_ftype(AHRS[mdl_idx].gyro_bias[i], -gyro_bias_limit, gyro_bias_limit);
|
||||
}
|
||||
}
|
||||
|
||||
// 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);
|
||||
|
||||
}
|
||||
|
|
@ -249,24 +249,24 @@ void EKFGSF_yaw::alignTilt()
|
|||
// 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_velocity;
|
||||
Vector3F down_in_bf = -delta_velocity;
|
||||
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);
|
||||
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;
|
||||
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 to create the transpose.
|
||||
Matrix3f R;
|
||||
Matrix3F R;
|
||||
for (uint8_t col=0; col<3; col++) {
|
||||
R[0][col] = north_in_bf[col];
|
||||
R[1][col] = east_in_bf[col];
|
||||
|
|
@ -284,9 +284,9 @@ void EKFGSF_yaw::alignYaw()
|
|||
{
|
||||
// Align yaw angle for each model
|
||||
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
|
||||
float roll,pitch,yaw;
|
||||
ftype roll,pitch,yaw;
|
||||
AHRS[mdl_idx].R.to_euler(&roll, &pitch, &yaw);
|
||||
|
||||
// set the yaw angle
|
||||
|
|
@ -297,7 +297,7 @@ void EKFGSF_yaw::alignYaw()
|
|||
|
||||
} else {
|
||||
// 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
|
||||
|
||||
// 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
|
||||
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
|
||||
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 {
|
||||
// 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
|
||||
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 float dvy = - del_vel_NED[0] * sinf(EKF[mdl_idx].X[2]) + del_vel_NED[1] * cosf(EKF[mdl_idx].X[2]);
|
||||
const Vector3F del_vel_NED = AHRS[mdl_idx].R * delta_velocity;
|
||||
const ftype dvx = del_vel_NED[0] * cosF(EKF[mdl_idx].X[2]) + del_vel_NED[1] * sinF(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:
|
||||
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
|
||||
// Compiler might be smart enough to optimise these out
|
||||
const float P00 = EKF[mdl_idx].P[0][0];
|
||||
const float P01 = EKF[mdl_idx].P[0][1];
|
||||
const float P02 = EKF[mdl_idx].P[0][2];
|
||||
const float P10 = EKF[mdl_idx].P[1][0];
|
||||
const float P11 = EKF[mdl_idx].P[1][1];
|
||||
const float P12 = EKF[mdl_idx].P[1][2];
|
||||
const float P20 = EKF[mdl_idx].P[2][0];
|
||||
const float P21 = EKF[mdl_idx].P[2][1];
|
||||
const float P22 = EKF[mdl_idx].P[2][2];
|
||||
const ftype P00 = EKF[mdl_idx].P[0][0];
|
||||
const ftype P01 = EKF[mdl_idx].P[0][1];
|
||||
const ftype P02 = EKF[mdl_idx].P[0][2];
|
||||
const ftype P10 = EKF[mdl_idx].P[1][0];
|
||||
const ftype P11 = EKF[mdl_idx].P[1][1];
|
||||
const ftype P12 = EKF[mdl_idx].P[1][2];
|
||||
const ftype P20 = EKF[mdl_idx].P[2][0];
|
||||
const ftype P21 = EKF[mdl_idx].P[2][1];
|
||||
const ftype P22 = EKF[mdl_idx].P[2][2];
|
||||
|
||||
// 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 float 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 dvxVar = sq(EKFGSF_accelNoise * velocity_dt); // variance of forward delta velocity - (m/s)^2
|
||||
const ftype dvyVar = dvxVar; // variance of right delta velocity - (m/s)^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 float t3 = cosf(EKF[mdl_idx].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 ftype t2 = sinF(EKF[mdl_idx].X[2]);
|
||||
const ftype t3 = cosF(EKF[mdl_idx].X[2]);
|
||||
const ftype t4 = dvy*t3;
|
||||
const ftype t5 = dvx*t2;
|
||||
const ftype t6 = t4+t5;
|
||||
const ftype t8 = P22*t6;
|
||||
const ftype t7 = P02-t8;
|
||||
const ftype t9 = dvx*t3;
|
||||
const ftype t11 = dvy*t2;
|
||||
const ftype t10 = t9-t11;
|
||||
const ftype t12 = dvxVar*t2*t3;
|
||||
const ftype t13 = t2*t2;
|
||||
const ftype t14 = t3*t3;
|
||||
const ftype t15 = P22*t10;
|
||||
const ftype t16 = P12+t15;
|
||||
|
||||
const float min_var = 1e-6f;
|
||||
EKF[mdl_idx].P[0][0] = fmaxf(P00-P20*t6+dvxVar*t14+dvyVar*t13-t6*t7, min_var);
|
||||
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][1] = P01+t12-P21*t6+t7*t10-dvyVar*t2*t3;
|
||||
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][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[2][0] = P20-t8;
|
||||
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
|
||||
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
|
||||
// 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
|
||||
EKF[mdl_idx].innov[0] = EKF[mdl_idx].X[0] - vel[0];
|
||||
EKF[mdl_idx].innov[1] = EKF[mdl_idx].X[1] - vel[1];
|
||||
|
||||
// copy covariance matrix to temporary variables
|
||||
const float P00 = EKF[mdl_idx].P[0][0];
|
||||
const float P01 = EKF[mdl_idx].P[0][1];
|
||||
const float P02 = EKF[mdl_idx].P[0][2];
|
||||
const float P10 = EKF[mdl_idx].P[1][0];
|
||||
const float P11 = EKF[mdl_idx].P[1][1];
|
||||
const float P12 = EKF[mdl_idx].P[1][2];
|
||||
const float P20 = EKF[mdl_idx].P[2][0];
|
||||
const float P21 = EKF[mdl_idx].P[2][1];
|
||||
const float P22 = EKF[mdl_idx].P[2][2];
|
||||
const ftype P00 = EKF[mdl_idx].P[0][0];
|
||||
const ftype P01 = EKF[mdl_idx].P[0][1];
|
||||
const ftype P02 = EKF[mdl_idx].P[0][2];
|
||||
const ftype P10 = EKF[mdl_idx].P[1][0];
|
||||
const ftype P11 = EKF[mdl_idx].P[1][1];
|
||||
const ftype P12 = EKF[mdl_idx].P[1][2];
|
||||
const ftype P20 = EKF[mdl_idx].P[2][0];
|
||||
const ftype P21 = EKF[mdl_idx].P[2][1];
|
||||
const ftype P22 = EKF[mdl_idx].P[2][2];
|
||||
|
||||
// calculate innovation variance
|
||||
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;
|
||||
|
||||
// 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]);
|
||||
float innov_comp_scale_factor = 1.0f;
|
||||
if (fabsf(S_det_inv) > 1E-6f) {
|
||||
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]);
|
||||
ftype innov_comp_scale_factor = 1.0f;
|
||||
if (fabsF(S_det_inv) > 1E-6f) {
|
||||
// Calculate elements for innovation covariance inverse matrix assuming symmetry
|
||||
S_det_inv = 1.0f / S_det_inv;
|
||||
const float 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 float S_inv_NE = EKF[mdl_idx].S[0][1] * S_det_inv;
|
||||
const ftype S_inv_NN = EKF[mdl_idx].S[1][1] * S_det_inv;
|
||||
const ftype S_inv_EE = EKF[mdl_idx].S[0][0] * 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]
|
||||
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
|
||||
// This protects from large measurement spikes
|
||||
if (test_ratio > 25.0f) {
|
||||
innov_comp_scale_factor = sqrtf(25.0f / test_ratio);
|
||||
innov_comp_scale_factor = sqrtF(25.0f / test_ratio);
|
||||
}
|
||||
} else {
|
||||
// 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
|
||||
// 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) {
|
||||
const ftype t2 = P00*velObsVar;
|
||||
const ftype t3 = P11*velObsVar;
|
||||
const ftype t4 = velObsVar*velObsVar;
|
||||
const ftype t5 = P00*P11;
|
||||
const ftype t9 = P01*P10;
|
||||
const ftype t6 = t2+t3+t4+t5-t9;
|
||||
ftype 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;
|
||||
float K[3][2];
|
||||
const ftype t8 = P11+velObsVar;
|
||||
const ftype t10 = P00+velObsVar;
|
||||
ftype K[3][2];
|
||||
|
||||
K[0][0] = -P01*P10*t7+P00*t7*t8;
|
||||
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][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 ftype t11 = P00*P01*t7;
|
||||
const ftype t15 = P01*t7*t10;
|
||||
const ftype t12 = t11-t15;
|
||||
const ftype t13 = P01*P10*t7;
|
||||
const ftype t16 = P00*t7*t8;
|
||||
const ftype t14 = t13-t16;
|
||||
const ftype t17 = t8*t12;
|
||||
const ftype t18 = P01*t14;
|
||||
const ftype t19 = t17+t18;
|
||||
const ftype t20 = t10*t14;
|
||||
const ftype t21 = P10*t12;
|
||||
const ftype t22 = t20+t21;
|
||||
const ftype t27 = P11*t7*t10;
|
||||
const ftype t23 = t13-t27;
|
||||
const ftype t24 = P10*P11*t7;
|
||||
const ftype t26 = P10*t7*t8;
|
||||
const ftype t25 = t24-t26;
|
||||
const ftype t28 = t8*t23;
|
||||
const ftype t29 = P01*t25;
|
||||
const ftype t30 = t28+t29;
|
||||
const ftype t31 = t10*t25;
|
||||
const ftype t32 = P10*t23;
|
||||
const ftype t33 = t31+t32;
|
||||
const ftype t34 = P01*P20*t7;
|
||||
const ftype t38 = P21*t7*t10;
|
||||
const ftype t35 = t34-t38;
|
||||
const ftype t36 = P10*P21*t7;
|
||||
const ftype t39 = P20*t7*t8;
|
||||
const ftype t37 = t36-t39;
|
||||
const ftype t40 = t8*t35;
|
||||
const ftype t41 = P01*t37;
|
||||
const ftype t42 = t40+t41;
|
||||
const ftype t43 = t10*t37;
|
||||
const ftype t44 = P10*t35;
|
||||
const ftype t45 = t43+t44;
|
||||
|
||||
const float min_var = 1e-6f;
|
||||
EKF[mdl_idx].P[0][0] = fmaxf(P00-t12*t19-t14*t22, min_var);
|
||||
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][1] = P01-t19*t23-t22*t25;
|
||||
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][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[2][0] = P20-t12*t42-t14*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
|
||||
forceSymmetry(mdl_idx);
|
||||
|
||||
// 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++) {
|
||||
// apply the state corrections including the compression scale factor
|
||||
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;
|
||||
}
|
||||
}
|
||||
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
|
||||
const float cos_yaw = cosf(yaw_delta);
|
||||
const float sin_yaw = sinf(yaw_delta);
|
||||
float R_prev[2][3];
|
||||
const ftype cos_yaw = cosF(yaw_delta);
|
||||
const ftype sin_yaw = sinF(yaw_delta);
|
||||
ftype R_prev[2][3];
|
||||
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][1] = R_prev[0][1] * cos_yaw - R_prev[1][1] * sin_yaw;
|
||||
|
|
@ -543,13 +543,13 @@ void EKFGSF_yaw::resetEKFGSF()
|
|||
run_ekf_gsf = false;
|
||||
|
||||
memset(&EKF, 0, sizeof(EKF));
|
||||
const float yaw_increment = M_2PI / (float)N_MODELS_EKFGSF;
|
||||
const ftype yaw_increment = M_2PI / (ftype)N_MODELS_EKFGSF;
|
||||
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
|
||||
// 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) + ((ftype)mdl_idx * yaw_increment);
|
||||
|
||||
// All filter models start with the same weight
|
||||
GSF.weights[mdl_idx] = 1.0f / (float)N_MODELS_EKFGSF;
|
||||
GSF.weights[mdl_idx] = 1.0f / (ftype)N_MODELS_EKFGSF;
|
||||
|
||||
// Use half yaw interval for yaw uncertainty as that is the maximum that the best model can be away from truth
|
||||
GSF.yaw_variance = sq(0.5f * yaw_increment);
|
||||
|
|
@ -558,48 +558,48 @@ void EKFGSF_yaw::resetEKFGSF()
|
|||
}
|
||||
|
||||
// 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 float t5 = EKF[mdl_idx].S[0][1] * EKF[mdl_idx].S[1][0];
|
||||
const float t3 = t2 - t5; // determinant
|
||||
const float t4 = 1.0f / MAX(t3, 1e-12f); // determinant inverse
|
||||
const ftype t2 = EKF[mdl_idx].S[0][0] * EKF[mdl_idx].S[1][1];
|
||||
const ftype t5 = EKF[mdl_idx].S[0][1] * EKF[mdl_idx].S[1][0];
|
||||
const ftype t3 = t2 - t5; // determinant
|
||||
const ftype t4 = 1.0f / MAX(t3, 1e-12f); // determinant inverse
|
||||
|
||||
// inv(S)
|
||||
float invMat[2][2];
|
||||
ftype invMat[2][2];
|
||||
invMat[0][0] = t4 * EKF[mdl_idx].S[1][1];
|
||||
invMat[1][1] = t4 * EKF[mdl_idx].S[0][0];
|
||||
invMat[0][1] = - t4 * EKF[mdl_idx].S[0][1];
|
||||
invMat[1][0] = - t4 * EKF[mdl_idx].S[1][0];
|
||||
|
||||
// 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[1] = invMat[1][0] * EKF[mdl_idx].innov[0] + invMat[1][1] * EKF[mdl_idx].innov[1];
|
||||
|
||||
// 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
|
||||
normDist = expf(-0.5f * normDist);
|
||||
normDist *= sqrtf(t4)/ M_2PI;
|
||||
normDist *= sqrtF(t4)/ M_2PI;
|
||||
return normDist;
|
||||
}
|
||||
|
||||
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]);
|
||||
float 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 P01 = 0.5f * (EKF[mdl_idx].P[0][1] + EKF[mdl_idx].P[1][0]);
|
||||
ftype P02 = 0.5f * (EKF[mdl_idx].P[0][2] + EKF[mdl_idx].P[2][0]);
|
||||
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][2] = EKF[mdl_idx].P[2][0] = P02;
|
||||
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
|
||||
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][1] += R[0][2] * g[0] - R[0][0] * g[2];
|
||||
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];
|
||||
|
||||
// Renormalise rows
|
||||
float rowLengthSq;
|
||||
ftype rowLengthSq;
|
||||
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];
|
||||
if (is_positive(rowLengthSq)) {
|
||||
// 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;
|
||||
Vector3f &row = ret[r];
|
||||
const ftype rowLengthInv = 1.5f - 0.5f * rowLengthSq;
|
||||
Vector3F &row = ret[r];
|
||||
row *= rowLengthInv;
|
||||
}
|
||||
}
|
||||
|
|
@ -625,7 +625,7 @@ Matrix3f EKFGSF_yaw::updateRotMat(const Matrix3f &R, const Vector3f &g) const
|
|||
return ret;
|
||||
}
|
||||
|
||||
bool EKFGSF_yaw::getYawData(float &yaw, float &yawVariance) const
|
||||
bool EKFGSF_yaw::getYawData(ftype &yaw, ftype &yawVariance) const
|
||||
{
|
||||
if (!vel_fuse_running) {
|
||||
return false;
|
||||
|
|
@ -635,14 +635,14 @@ bool EKFGSF_yaw::getYawData(float &yaw, float &yawVariance) const
|
|||
return true;
|
||||
}
|
||||
|
||||
bool EKFGSF_yaw::getVelInnovLength(float &velInnovLength) const
|
||||
bool EKFGSF_yaw::getVelInnovLength(ftype &velInnovLength) const
|
||||
{
|
||||
if (!vel_fuse_running) {
|
||||
return false;
|
||||
}
|
||||
velInnovLength = 0.0f;
|
||||
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;
|
||||
}
|
||||
|
|
@ -650,6 +650,6 @@ bool EKFGSF_yaw::getVelInnovLength(float &velInnovLength) const
|
|||
void EKFGSF_yaw::setGyroBias(Vector3f &gyroBias)
|
||||
{
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -16,28 +16,28 @@ public:
|
|||
EKFGSF_yaw();
|
||||
|
||||
// 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)
|
||||
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 float delVelDT, // time interval that delVel was integrated over (sec) - must be no less than IMU_DT_MIN_SEC
|
||||
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 ftype delAngDT, // time interval that delAng 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
|
||||
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
|
||||
// Should be called after update(...) whenever new velocity data is available
|
||||
void fuseVelData(const Vector2f &vel, // NE velocity measurement (m/s)
|
||||
const float velAcc); // 1-sigma accuracy of velocity measurement (m/s)
|
||||
void fuseVelData(const Vector2F &vel, // NE velocity measurement (m/s)
|
||||
const ftype velAcc); // 1-sigma accuracy of velocity measurement (m/s)
|
||||
|
||||
// set the gyro bias in rad/sec
|
||||
void setGyroBias(Vector3f &gyroBias);
|
||||
|
||||
// get yaw estimated and corresponding variance
|
||||
// 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
|
||||
// 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
|
||||
// IDs of the messages to log, e.g. LOG_NKY0_MSG, LOG_NKY1_MSG
|
||||
|
|
@ -45,7 +45,6 @@ public:
|
|||
|
||||
private:
|
||||
|
||||
typedef float ftype;
|
||||
#if MATH_CHECK_INDEXES
|
||||
typedef VectorN<ftype,2> Vector2;
|
||||
typedef VectorN<ftype,3> Vector3;
|
||||
|
|
@ -58,40 +57,40 @@ private:
|
|||
#endif
|
||||
|
||||
// Parameters
|
||||
const float EKFGSF_gyroNoise{1.0e-1f}; // 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 float EKFGSF_tiltGain{0.2f}; // 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 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_gyroNoise{1.0e-1}; // yaw rate noise used for covariance prediction (rad/sec)
|
||||
const ftype EKFGSF_accelNoise{2.0}; // horizontal accel noise used for covariance prediction (m/sec**2)
|
||||
const ftype EKFGSF_tiltGain{0.2}; // gain from tilt error to 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 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
|
||||
// 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.
|
||||
Vector3f delta_angle;
|
||||
Vector3f delta_velocity;
|
||||
float angle_dt;
|
||||
float velocity_dt;
|
||||
Vector3F delta_angle;
|
||||
Vector3F delta_velocity;
|
||||
ftype angle_dt;
|
||||
ftype velocity_dt;
|
||||
struct ahrs_struct {
|
||||
Matrix3f 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 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)
|
||||
Matrix3F 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; // true when AHRS has been aligned
|
||||
ftype accel_FR[2]; // front-right acceleration vector in a horizontal plane (m/s/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
|
||||
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];
|
||||
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
|
||||
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)
|
||||
float true_airspeed; // true airspeed used to correct for centripetal acceleratoin in coordinated turns (m/s)
|
||||
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)
|
||||
ftype ahrs_accel_norm; // length of body frame specific force vector used by AHRS calculation (m/s/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
|
||||
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
|
||||
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
|
||||
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.
|
||||
|
||||
struct EKF_struct {
|
||||
float X[3]; // Vel North (m/s), Vel East (m/s), yaw (rad)
|
||||
float P[3][3]; // covariance matrix
|
||||
float S[2][2]; // N,E velocity innovation variance (m/s)^2
|
||||
float innov[2]; // Velocity N,E innovation (m/s)
|
||||
ftype X[3]; // Vel North (m/s), Vel East (m/s), yaw (rad)
|
||||
ftype P[3][3]; // covariance matrix
|
||||
ftype S[2][2]; // N,E velocity innovation variance (m/s)^2
|
||||
ftype innov[2]; // Velocity N,E innovation (m/s)
|
||||
};
|
||||
EKF_struct EKF[N_MODELS_EKFGSF];
|
||||
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
|
||||
// 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
|
||||
void forceSymmetry(const uint8_t mdl_idx);
|
||||
|
|
@ -128,13 +127,13 @@ private:
|
|||
// EKF's to form a single state estimate.
|
||||
|
||||
struct GSF_struct {
|
||||
float yaw; // yaw (rad)
|
||||
float yaw_variance; // Yaw state variance (rad^2)
|
||||
float weights[N_MODELS_EKFGSF]; // Weighting applied to each EKF model. Sum of weights is unity.
|
||||
ftype yaw; // yaw (rad)
|
||||
ftype yaw_variance; // Yaw state variance (rad^2)
|
||||
ftype weights[N_MODELS_EKFGSF]; // Weighting applied to each EKF model. Sum of weights is unity.
|
||||
};
|
||||
GSF_struct GSF;
|
||||
|
||||
// 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
|
||||
float gaussianDensity(const uint8_t mdl_idx) const;
|
||||
ftype gaussianDensity(const uint8_t mdl_idx) const;
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue