diff --git a/EKF/EKFGSF_yaw.h b/EKF/EKFGSF_yaw.h index 3b44101f18..07b8061ebc 100644 --- a/EKF/EKFGSF_yaw.h +++ b/EKF/EKFGSF_yaw.h @@ -60,25 +60,25 @@ private: // Declarations used by the bank of N_MODELS_EKFGSF AHRS complementary filters - Vector3f _delta_ang; // IMU delta angle (rad) - Vector3f _delta_vel; // IMU delta velocity (m/s) - float _delta_ang_dt; // _delta_ang integration time interval (sec) - float _delta_vel_dt; // _delta_vel integration time interval (sec) - float _true_airspeed; // true airspeed used for centripetal accel compensation (m/s) + Vector3f _delta_ang{}; // IMU delta angle (rad) + Vector3f _delta_vel{}; // IMU delta velocity (m/s) + float _delta_ang_dt{}; // _delta_ang integration time interval (sec) + float _delta_vel_dt{}; // _delta_vel integration time interval (sec) + float _true_airspeed{}; // true airspeed used for centripetal accel compensation (m/s) struct _ahrs_ekf_gsf_struct{ - Dcmf R; // matrix that rotates a vector from body to earth frame - Vector3f gyro_bias; // gyro bias learned and used by the quaternion calculation - bool aligned{false}; // true when AHRS has been aligned - float vel_NE[2] {}; // NE velocity vector from last GPS measurement (m/s) - bool fuse_gps = false; // true when GPS should be fused on that frame - float accel_dt = 0; // time step used when generating _simple_accel_FR data (sec) - }; - _ahrs_ekf_gsf_struct _ahrs_ekf_gsf[N_MODELS_EKFGSF]; - bool _ahrs_ekf_gsf_tilt_aligned = false;// true the initial tilt alignment has been calculated - float _ahrs_accel_fusion_gain; // gain from accel vector tilt error to rate gyro correction used by AHRS calculation - Vector3f _ahrs_accel; // low pass filtered body frame specific force vector used by AHRS calculation (m/s/s) - float _ahrs_accel_norm; // length of _ahrs_accel specific force vector used by AHRS calculation (m/s/s) + Dcmf R; // matrix that rotates a vector from body to earth frame + Vector3f gyro_bias; // gyro bias learned and used by the quaternion calculation + bool aligned; // true when AHRS has been aligned + float 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) + } _ahrs_ekf_gsf[N_MODELS_EKFGSF]{}; + + bool _ahrs_ekf_gsf_tilt_aligned{}; // true the initial tilt alignment has been calculated + float _ahrs_accel_fusion_gain{}; // gain from accel vector tilt error to rate gyro correction used by AHRS calculation + Vector3f _ahrs_accel{}; // low pass filtered body frame specific force vector used by AHRS calculation (m/s/s) + float _ahrs_accel_norm{}; // length of _ahrs_accel specific force vector used by AHRS calculation (m/s/s) // calculate the gain from gravity vector misalingment to tilt correction to be used by all AHRS filters float ahrsCalcAccelGain() const; @@ -98,18 +98,18 @@ private: // Declarations used by a bank of N_MODELS_EKFGSF EKFs struct _ekf_gsf_struct{ - matrix::Vector3f X; // Vel North (m/s), Vel East (m/s), yaw (rad)s - matrix::SquareMatrix P; // covariance matrix - matrix::SquareMatrix S_inverse; // inverse of the innovation covariance matrix - float S_det_inverse; // inverse of the innovation covariance matrix determinant - matrix::Vector2f innov; // Velocity N,E innovation (m/s) - }; - _ekf_gsf_struct _ekf_gsf[N_MODELS_EKFGSF]; - bool _vel_data_updated; // true when velocity data has been updated - bool _run_ekf_gsf; // true when operating condition is suitable for to run the GSF and EKF models and fuse velocity data - Vector2f _vel_NE; // NE velocity observations (m/s) - float _vel_accuracy; // 1-sigma accuracy of velocity observations (m/s) - bool _ekf_gsf_vel_fuse_started; // true when the EKF's have started fusing velocity data and the prediction and update processing is active + matrix::Vector3f X; // Vel North (m/s), Vel East (m/s), yaw (rad)s + matrix::SquareMatrix P; // covariance matrix + matrix::SquareMatrix S_inverse; // inverse of the innovation covariance matrix + float S_det_inverse; // inverse of the innovation covariance matrix determinant + matrix::Vector2f innov; // Velocity N,E innovation (m/s) + } _ekf_gsf[N_MODELS_EKFGSF]{}; + + bool _vel_data_updated{}; // true when velocity data has been updated + bool _run_ekf_gsf{}; // true when operating condition is suitable for to run the GSF and EKF models and fuse velocity data + Vector2f _vel_NE{}; // NE velocity observations (m/s) + float _vel_accuracy{}; // 1-sigma accuracy of velocity observations (m/s) + bool _ekf_gsf_vel_fuse_started{}; // true when the EKF's have started fusing velocity data and the prediction and update processing is active // initialise states and covariance data for the GSF and EKF filters void initialiseEKFGSF(); @@ -133,9 +133,9 @@ private: // Declarations used by the Gaussian Sum Filter (GSF) that combines the individual EKF yaw estimates - matrix::Vector _model_weights; - float _gsf_yaw; // yaw estimate (rad) - float _gsf_yaw_variance; // variance of yaw estimate (rad^2) + matrix::Vector _model_weights{}; + float _gsf_yaw{}; // yaw estimate (rad) + float _gsf_yaw_variance{}; // variance of yaw estimate (rad^2) // return the probability of the state estimate for the specified EKF assuming a gaussian error distribution float gaussianDensity(const uint8_t model_index) const;