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

667 lines
27 KiB
C++
Raw Permalink Blame History

/*
Definition of functions used to provide a backup estimate of yaw angle
that doesn't use a magnetometer. Uses a bank of 3-state EKF's organised
as a Guassian sum filter where states are velocity N,E (m/s) and yaw angle (rad)
Written by Paul Riseborough <p_riseborough@live.com.au>
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#include "AP_NavEKF/EKFGSF_yaw.h"
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <GCS_MAVLink/GCS.h>
EKFGSF_yaw::EKFGSF_yaw() {};
void EKFGSF_yaw::update(const Vector3F &delAng,
const Vector3F &delVel,
const ftype delAngDT,
const ftype delVelDT,
bool runEKF,
ftype TAS)
{
// copy to class variables
delta_angle = delAng;
delta_velocity = delVel;
angle_dt = delAngDT;
velocity_dt = delVelDT;
run_ekf_gsf = runEKF;
true_airspeed = TAS;
// 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 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 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) {
alignTilt();
ahrs_tilt_aligned = true;
ahrs_accel = accel;
}
return;
}
// Calculate common variables used by the AHRS prediction models
ahrs_accel_norm = ahrs_accel.length();
// 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.
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
// Gain varies from unity at 1g to zero at 2g
accel_gain = EKFGSF_tiltGain * sq(2.0f - EKFGSF_ahrs_ng);
} else if (accel_gain <= 1.5f) {
// Gain varies from unity at 1g to zero at 1.5g
accel_gain = EKFGSF_tiltGain * sq(3.0f - 2.0f * EKFGSF_ahrs_ng);
} else {
// Gain is zero above max g
accel_gain = 0.0f;
}
} else if (accel_gain > 0.5f) {
// Gain varies from zero at 0.5g to unity at 1g
accel_gain = EKFGSF_tiltGain * sq(2.0f * EKFGSF_ahrs_ng - 1.0f);
} else {
// Gain is zero below min g
accel_gain = 0.0f;
}
// Always run the AHRS prediction cycle for each model
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
predict(mdl_idx);
}
if (vel_fuse_running && !run_ekf_gsf) {
vel_fuse_running = false;
}
// 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 = {};
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]);
}
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++) {
ftype delta[3];
for (uint8_t row = 0; row < 3; row++) {
delta[row] = EKF[mdl_idx].X[row] - GSF.X[row];
}
for (uint8_t row = 0; row < 3; row++) {
for (uint8_t col = 0; col < 3; col++) {
GSF.P[row][col] += GSF.weights[mdl_idx] * (EKF[mdl_idx].P[row][col] + delta[row] * delta[col]);
}
}
}
*/
GSF.yaw_variance = 0.0f;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
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 ftype velAcc)
{
// convert reported accuracy to a variance, but limit lower value to protect algorithm stability
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) {
if (!vel_fuse_running) {
// Perform in-flight alignment
resetEKFGSF();
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
// Use the firstGPS measurement to set the velocities and corresponding variances
EKF[mdl_idx].X[0] = vel[0];
EKF[mdl_idx].X[1] = vel[1];
EKF[mdl_idx].P[0][0] = velObsVar;
EKF[mdl_idx].P[1][1] = velObsVar;
}
alignYaw();
vel_fuse_running = true;
} else {
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
if (!correct(mdl_idx, vel, velObsVar)) {
state_update_failed = true;
}
}
if (!state_update_failed) {
// Calculate weighting for each model assuming a normal error distribution
const ftype min_weight = 1e-5f;
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];
if (newWeight[mdl_idx] < min_weight) {
n_clips++;
newWeight[mdl_idx] = min_weight;
}
total_w += newWeight[mdl_idx];
}
// 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) {
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;
}
} else {
resetEKFGSF();
}
}
}
}
}
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]);
// Calculate angular rate vector in rad/sec averaged across last sample interval
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)
if (accel_gain > 0.0f) {
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);
// Correct measured accel for centripetal acceleration
accel -= centripetal_accel_vec_bf;
}
tilt_error_gyro_correction = (k % accel) * (accel_gain / ahrs_accel_norm);
}
// Gyro bias estimation
const ftype gyro_bias_limit = radians(5.0f);
const ftype spinRate_squared = ang_rate_delayed_raw.length_squared();
if (spinRate_squared < sq(0.175f)) {
AHRS[mdl_idx].gyro_bias -= tilt_error_gyro_correction * (EKFGSF_gyroBiasGain * angle_dt);
// sanity check
if (AHRS[mdl_idx].gyro_bias.is_nan()) {
AHRS[mdl_idx].gyro_bias.zero();
}
for (uint8_t i = 0; i < 3; i++) {
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;
AHRS[mdl_idx].R = updateRotMat(AHRS[mdl_idx].R, ahrs_delta_angle);
}
void EKFGSF_yaw::alignTilt()
{
// Rotation matrix is constructed directly from acceleration measurement and will be the same for
// all models so only need to calculate it once. Assumptions are:
// 1) Yaw angle is zero - yaw is aligned later for each model when velocity fusion commences.
// 2) The vehicle is not accelerating so all of the measured acceleration is due to gravity.
// Calculate earth frame Down axis unit vector rotated into body frame
Vector3F down_in_bf = -delta_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);
north_in_bf.normalize();
// Calculate earth frame East axis unit vector rotated into body frame, orthogonal to 'down_in_bf' and 'north_in_bf'
// % operator is overloaded to provide a cross product
const Vector3F east_in_bf = down_in_bf % north_in_bf;
// Each column in a rotation matrix from earth frame to body frame represents the projection of the
// corresponding earth frame unit vector rotated into the body frame, eg 'north_in_bf' would be the first column.
// We need the rotation matrix from body frame to earth frame so the earth frame unit vectors rotated into body
// frame are copied into corresponding rows instead to create the transpose.
Matrix3F R;
for (uint8_t col=0; col<3; col++) {
R[0][col] = north_in_bf[col];
R[1][col] = east_in_bf[col];
R[2][col] = down_in_bf[col];
}
// record alignment
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
AHRS[mdl_idx].R = R;
AHRS[mdl_idx].aligned = true;
}
}
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])) {
// get the roll, pitch, yaw estimates from the rotation matrix using a 321 Tait-Bryan rotation sequence
ftype roll,pitch,yaw;
AHRS[mdl_idx].R.to_euler(&roll, &pitch, &yaw);
// set the yaw angle
yaw = wrap_PI(EKF[mdl_idx].X[2]);
// update the body to earth frame rotation matrix
AHRS[mdl_idx].R.from_euler(roll, pitch, yaw);
} else {
// Calculate the 312 Tait-Bryan rotation sequence that rotates from earth to body frame
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
AHRS[mdl_idx].R.from_euler312(euler312[0], euler312[1], euler312[2]);
}
}
}
// predict states and covariance for specified model index
void EKFGSF_yaw::predict(const uint8_t mdl_idx)
{
// generate an attitude reference using IMU data
predictAHRS(mdl_idx);
// we don't start running the EKF part of the algorithm until there are regular velocity observations
if (!vel_fuse_running) {
return;
}
// 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])) {
// 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]);
} 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)
}
// calculate delta velocity in a horizontal front-right frame
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];
EKF[mdl_idx].X[1] += del_vel_NED[1];
// predict covariance - autocode from https://github.com/priseborough/3_state_filter/blob/flightLogReplay-wip/calcPupdate.txt
// Local short variable name copies required for readability
// Compiler might be smart enough to optimise these out
const 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 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 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 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][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);
// force symmetry
forceSymmetry(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 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 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;
EKF[mdl_idx].S[1][1] = P11 + velObsVar;
EKF[mdl_idx].S[0][1] = P01;
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
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 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 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);
}
} else {
// skip this fusion step because calculation is badly conditioned
return false;
}
// 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 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 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;
K[1][0] = -P10*P11*t7+P10*t7*t8;
K[1][1] = -P01*P10*t7+P11*t7*t10;
K[2][0] = -P10*P21*t7+P20*t7*t8;
K[2][1] = -P01*P20*t7+P21*t7*t10;
const 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 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][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);
// force symmetry
forceSymmetry(mdl_idx);
// Apply state corrections and capture change in yaw angle
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 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 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;
AHRS[mdl_idx].R[0][2] = R_prev[0][2] * cos_yaw - R_prev[1][2] * sin_yaw;
AHRS[mdl_idx].R[1][0] = R_prev[0][0] * sin_yaw + R_prev[1][0] * cos_yaw;
AHRS[mdl_idx].R[1][1] = R_prev[0][1] * sin_yaw + R_prev[1][1] * cos_yaw;
AHRS[mdl_idx].R[1][2] = R_prev[0][2] * sin_yaw + R_prev[1][2] * cos_yaw;
return true;
}
void EKFGSF_yaw::resetEKFGSF()
{
memset(&GSF, 0, sizeof(GSF));
vel_fuse_running = false;
run_ekf_gsf = false;
memset(&EKF, 0, sizeof(EKF));
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) + ((ftype)mdl_idx * yaw_increment);
// All filter models start with the same weight
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);
EKF[mdl_idx].P[2][2] = GSF.yaw_variance;
}
}
// returns the probability of a selected model output assuming a gaussian error distribution
ftype EKFGSF_yaw::gaussianDensity(const uint8_t mdl_idx) const
{
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)
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
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
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;
return normDist;
}
void EKFGSF_yaw::forceSymmetry(const uint8_t mdl_idx)
{
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 ret = R;
ret[0][0] += R[0][1] * g[2] - R[0][2] * g[1];
ret[0][1] += R[0][2] * g[0] - R[0][0] * g[2];
ret[0][2] += R[0][0] * g[1] - R[0][1] * g[0];
ret[1][0] += R[1][1] * g[2] - R[1][2] * g[1];
ret[1][1] += R[1][2] * g[0] - R[1][0] * g[2];
ret[1][2] += R[1][0] * g[1] - R[1][1] * g[0];
ret[2][0] += R[2][1] * g[2] - R[2][2] * g[1];
ret[2][1] += R[2][2] * g[0] - R[2][0] * g[2];
ret[2][2] += R[2][0] * g[1] - R[2][1] * g[0];
// Renormalise rows
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 ftype rowLengthInv = 1.5f - 0.5f * rowLengthSq;
Vector3F &row = ret[r];
row *= rowLengthInv;
}
}
return ret;
}
// returns true if a yaw estimate is available. yaw and its variance
// is returned, as well as the number of models which are *not* being
// used to snthesise the yaw.
bool EKFGSF_yaw::getYawData(ftype &yaw, ftype &yawVariance, uint8_t *_n_clips) const
{
if (!vel_fuse_running) {
return false;
}
yaw = GSF.yaw;
yawVariance = GSF.yaw_variance;
if (_n_clips != nullptr) {
*_n_clips = n_clips;
}
return true;
}
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])));
}
return true;
}
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.toftype();
}
}
Reference in New Issue View Git Blame Copy Permalink