diff --git a/libraries/AP_NavEKF/AP_NavEKF_core_common.cpp b/libraries/AP_NavEKF/AP_NavEKF_core_common.cpp index f5607c7c46..5eb04b41cb 100644 --- a/libraries/AP_NavEKF/AP_NavEKF_core_common.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF_core_common.cpp @@ -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 } diff --git a/libraries/AP_NavEKF/AP_NavEKF_core_common.h b/libraries/AP_NavEKF/AP_NavEKF_core_common.h index 4c0d6c19ef..253467cf0d 100644 --- a/libraries/AP_NavEKF/AP_NavEKF_core_common.h +++ b/libraries/AP_NavEKF/AP_NavEKF_core_common.h @@ -19,6 +19,7 @@ #include #include #include +#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 Vector28; typedef VectorN,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 + diff --git a/libraries/AP_NavEKF/AP_Nav_Common.h b/libraries/AP_NavEKF/AP_Nav_Common.h index 90e2db538d..83e1941939 100644 --- a/libraries/AP_NavEKF/AP_Nav_Common.h +++ b/libraries/AP_NavEKF/AP_Nav_Common.h @@ -17,6 +17,7 @@ #pragma once #include +#include union nav_filter_status { struct { diff --git a/libraries/AP_NavEKF/EKFGSF_Logging.cpp b/libraries/AP_NavEKF/EKFGSF_Logging.cpp index 541da91918..31cc095d92 100644 --- a/libraries/AP_NavEKF/EKFGSF_Logging.cpp +++ b/libraries/AP_NavEKF/EKFGSF_Logging.cpp @@ -2,6 +2,8 @@ #include +#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], diff --git a/libraries/AP_NavEKF/EKFGSF_yaw.cpp b/libraries/AP_NavEKF/EKFGSF_yaw.cpp index 2477228eff..e104ee499a 100644 --- a/libraries/AP_NavEKF/EKFGSF_yaw.cpp +++ b/libraries/AP_NavEKF/EKFGSF_yaw.cpp @@ -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,8 +225,8 @@ 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); @@ -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 - 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,7 +543,7 @@ 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 / (float)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); @@ -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(); } } diff --git a/libraries/AP_NavEKF/EKFGSF_yaw.h b/libraries/AP_NavEKF/EKFGSF_yaw.h index 1887ae559b..4a333ef2eb 100644 --- a/libraries/AP_NavEKF/EKFGSF_yaw.h +++ b/libraries/AP_NavEKF/EKFGSF_yaw.h @@ -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 Vector2; typedef VectorN 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; };