zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #266 from dagar/valgrind 

ekf2 initialization fixes
master
Paul Riseborough 8 years ago committed by GitHub
parent
c62bdcbdb2 3919d60f66
commit
8a2c5c1ad2
6 changed files with 384 additions and 601 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 306
      EKF/common.h
  2. 149
      EKF/ekf.cpp
  3. 198
      EKF/ekf.h
  4. 131
      EKF/ekf_helper.cpp
  5. 82
      EKF/estimator_interface.cpp
  6. 115
      EKF/estimator_interface.h

306
EKF/common.h
Unescape View File

@ -108,8 +108,8 @@ struct magSample { @@ -108,8 +108,8 @@ struct magSample {
};
struct baroSample {
float hgt; // barometer height above sea level measurement in m
uint64_t time_us; // timestamp in microseconds
float hgt{0.0f}; // barometer height above sea level measurement in m
uint64_t time_us{0}; // timestamp in microseconds
};
struct rangeSample {
@ -182,92 +182,92 @@ struct dragSample { @@ -182,92 +182,92 @@ struct dragSample {
struct parameters {
// measurement source control
int fusion_mode; // bitmasked integer that selects which aiding sources will be used
int vdist_sensor_type; // selects the primary source for height data
int sensor_interval_min_ms; // minimum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers.
int fusion_mode{MASK_USE_GPS}; // bitmasked integer that selects which aiding sources will be used
int vdist_sensor_type{VDIST_SENSOR_BARO}; // selects the primary source for height data
int sensor_interval_min_ms{20}; // minimum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers.
// measurement time delays
float mag_delay_ms; // magnetometer measurement delay relative to the IMU (msec)
float baro_delay_ms; // barometer height measurement delay relative to the IMU (msec)
float gps_delay_ms; // GPS measurement delay relative to the IMU (msec)
float airspeed_delay_ms; // airspeed measurement delay relative to the IMU (msec)
float flow_delay_ms; // optical flow measurement delay relative to the IMU (msec) - this is to the middle of the optical flow integration interval
float range_delay_ms; // range finder measurement delay relative to the IMU (msec)
float ev_delay_ms; // off-board vision measurement delay relative to the IMU (msec)
float mag_delay_ms{0.0f}; // magnetometer measurement delay relative to the IMU (msec)
float baro_delay_ms{0.0f}; // barometer height measurement delay relative to the IMU (msec)
float gps_delay_ms{200.0f}; // GPS measurement delay relative to the IMU (msec)
float airspeed_delay_ms{200.0f}; // airspeed measurement delay relative to the IMU (msec)
float flow_delay_ms{5.0f}; // optical flow measurement delay relative to the IMU (msec) - this is to the middle of the optical flow integration interval
float range_delay_ms{5.0f}; // range finder measurement delay relative to the IMU (msec)
float ev_delay_ms{100.0f}; // off-board vision measurement delay relative to the IMU (msec)
// input noise
float gyro_noise; // IMU angular rate noise used for covariance prediction (rad/sec)
float accel_noise; // IMU acceleration noise use for covariance prediction (m/sec/sec)
float gyro_noise{1.5e-2f}; // IMU angular rate noise used for covariance prediction (rad/sec)
float accel_noise{3.5e-1f}; // IMU acceleration noise use for covariance prediction (m/sec/sec)
// process noise
float gyro_bias_p_noise; // process noise for IMU rate gyro bias prediction (rad/sec**2)
float accel_bias_p_noise; // process noise for IMU accelerometer bias prediction (m/sec**3)
float mage_p_noise; // process noise for earth magnetic field prediction (Guass/sec)
float magb_p_noise; // process noise for body magnetic field prediction (Guass/sec)
float wind_vel_p_noise; // process noise for wind velocity prediction (m/sec/sec)
float terrain_p_noise; // process noise for terrain offset (m/sec)
float terrain_gradient; // gradient of terrain used to estimate process noise due to changing position (m/m)
// initialisation errors
float switch_on_gyro_bias; // 1-sigma gyro bias uncertainty at switch on (rad/sec)
float switch_on_accel_bias; // 1-sigma accelerometer bias uncertainty at switch on (m/s**2)
float initial_tilt_err; // 1-sigma tilt error after initial alignment using gravity vector (rad)
float gyro_bias_p_noise{1.0e-3f}; // process noise for IMU rate gyro bias prediction (rad/sec**2)
float accel_bias_p_noise{3.0e-3f}; // process noise for IMU accelerometer bias prediction (m/sec**3)
float mage_p_noise{1.0e-3f}; // process noise for earth magnetic field prediction (Guass/sec)
float magb_p_noise{1.0e-4}; // process noise for body magnetic field prediction (Guass/sec)
float wind_vel_p_noise{1.0e-1f}; // process noise for wind velocity prediction (m/sec/sec)
float terrain_p_noise{5.0f}; // process noise for terrain offset (m/sec)
float terrain_gradient{0.5f}; // gradient of terrain used to estimate process noise due to changing position (m/m)
// initialization errors
float switch_on_gyro_bias{0.1f}; // 1-sigma gyro bias uncertainty at switch on (rad/sec)
float switch_on_accel_bias{0.2f}; // 1-sigma accelerometer bias uncertainty at switch on (m/s**2)
float initial_tilt_err{0.1f}; // 1-sigma tilt error after initial alignment using gravity vector (rad)
// position and velocity fusion
float gps_vel_noise; // observation noise for gps velocity fusion (m/sec)
float gps_pos_noise; // observation noise for gps position fusion (m)
float pos_noaid_noise; // observation noise for non-aiding position fusion (m)
float baro_noise; // observation noise for barometric height fusion (m)
float baro_innov_gate; // barometric height innovation consistency gate size (STD)
float posNE_innov_gate; // GPS horizontal position innovation consistency gate size (STD)
float vel_innov_gate; // GPS velocity innovation consistency gate size (STD)
float hgt_reset_lim; // The maximum 1-sigma uncertainty in height that can be tolerated before the height state is reset (m)
float gps_vel_noise{5.0e-1f}; // observation noise for gps velocity fusion (m/sec)
float gps_pos_noise{0.5f}; // observation noise for gps position fusion (m)
float pos_noaid_noise{10.0f}; // observation noise for non-aiding position fusion (m)
float baro_noise{2.0f}; // observation noise for barometric height fusion (m)
float baro_innov_gate{5.0f}; // barometric height innovation consistency gate size (STD)
float posNE_innov_gate{5.0f}; // GPS horizontal position innovation consistency gate size (STD)
float vel_innov_gate{5.0f}; // GPS velocity innovation consistency gate size (STD)
float hgt_reset_lim{0.0f}; // The maximum 1-sigma uncertainty in height that can be tolerated before the height state is reset (m)
// magnetometer fusion
float mag_heading_noise; // measurement noise used for simple heading fusion (rad)
float mag_noise; // measurement noise used for 3-axis magnetoemeter fusion (Gauss)
float mag_declination_deg; // magnetic declination (degrees)
float heading_innov_gate; // heading fusion innovation consistency gate size (STD)
float mag_innov_gate; // magnetometer fusion innovation consistency gate size (STD)
int mag_declination_source; // bitmask used to control the handling of declination data
int mag_fusion_type; // integer used to specify the type of magnetometer fusion used
float mag_heading_noise{3.0e-1f}; // measurement noise used for simple heading fusion (rad)
float mag_noise{5.0e-2f}; // measurement noise used for 3-axis magnetoemeter fusion (Gauss)
float mag_declination_deg{0.0f}; // magnetic declination (degrees)
float heading_innov_gate{2.6f}; // heading fusion innovation consistency gate size (STD)
float mag_innov_gate{3.0f}; // magnetometer fusion innovation consistency gate size (STD)
int mag_declination_source{7}; // bitmask used to control the handling of declination data
int mag_fusion_type{0}; // integer used to specify the type of magnetometer fusion used
// airspeed fusion
float tas_innov_gate; // True Airspeed Innovation consistency gate size in standard deciation
float eas_noise; // EAS measurement noise standard deviation used for airspeed fusion [m/s]
float tas_innov_gate{5.0f}; // True Airspeed Innovation consistency gate size in standard deciation
float eas_noise{1.4f}; // EAS measurement noise standard deviation used for airspeed fusion [m/s]
// synthetic sideslip fusion
float beta_innov_gate; // synthetic sideslip innovation consistency gate size in standard deviation (STD)
float beta_noise; // synthetic sideslip noise (rad)
float beta_avg_ft_us; // The average time between synthetic sideslip measurements (usec)
// synthetic sideslip fusion
float beta_innov_gate{5.0f}; // synthetic sideslip innovation consistency gate size in standard deviation (STD)
float beta_noise{0.3f}; // synthetic sideslip noise (rad)
float beta_avg_ft_us{1000000.0f}; // The average time between synthetic sideslip measurements (usec)
// range finder fusion
float range_noise; // observation noise for range finder measurements (m)
float range_innov_gate; // range finder fusion innovation consistency gate size (STD)
float rng_gnd_clearance; // minimum valid value for range when on ground (m)
float rng_sens_pitch; // Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
float range_noise_scaler; // scaling from range measurement to noise (m/m)
float range_noise{0.1f}; // observation noise for range finder measurements (m)
float range_innov_gate{5.0f}; // range finder fusion innovation consistency gate size (STD)
float rng_gnd_clearance{0.1f}; // minimum valid value for range when on ground (m)
float rng_sens_pitch{0.0f}; // Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
float range_noise_scaler{0.0f}; // scaling from range measurement to noise (m/m)
// vision position fusion
float ev_innov_gate; // vision estimator fusion innovation consistency gate size (STD)
float ev_innov_gate{5.0f}; // vision estimator fusion innovation consistency gate size (STD)
// optical flow fusion
float flow_noise; // observation noise for optical flow LOS rate measurements (rad/sec)
float flow_noise_qual_min; // observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
int flow_qual_min; // minimum acceptable quality integer from the flow sensor
float flow_innov_gate; // optical flow fusion innovation consistency gate size (STD)
float flow_rate_max; // maximum valid optical flow rate (rad/sec)
float flow_noise{0.15f}; // observation noise for optical flow LOS rate measurements (rad/sec)
float flow_noise_qual_min{0.5f}; // observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
int flow_qual_min{1}; // minimum acceptable quality integer from the flow sensor
float flow_innov_gate{3.0f}; // optical flow fusion innovation consistency gate size (STD)
float flow_rate_max{2.5f}; // maximum valid optical flow rate (rad/sec)
// these parameters control the strictness of GPS quality checks used to determine uf the GPS is
// good enough to set a local origin and commence aiding
int gps_check_mask; // bitmask used to control which GPS quality checks are used
float req_hacc; // maximum acceptable horizontal position error
float req_vacc; // maximum acceptable vertical position error
float req_sacc; // maximum acceptable speed error
int req_nsats; // minimum acceptable satellite count
float req_gdop; // maximum acceptable geometric dilution of precision
float req_hdrift; // maximum acceptable horizontal drift speed
float req_vdrift; // maximum acceptable vertical drift speed
int gps_check_mask{21}; // bitmask used to control which GPS quality checks are used
float req_hacc{5.0f}; // maximum acceptable horizontal position error
float req_vacc{8.0f}; // maximum acceptable vertical position error
float req_sacc{1.0f}; // maximum acceptable speed error
int req_nsats{6}; // minimum acceptable satellite count
float req_gdop{2.0f}; // maximum acceptable geometric dilution of precision
float req_hdrift{0.3f}; // maximum acceptable horizontal drift speed
float req_vdrift{0.5f}; // maximum acceptable vertical drift speed
// XYZ offset of sensors in body axes (m)
Vector3f imu_pos_body; // xyz position of IMU in body frame (m)
@ -277,142 +277,24 @@ struct parameters { @@ -277,142 +277,24 @@ struct parameters {
Vector3f ev_pos_body; // xyz position of VI-sensor focal point in body frame (m)
// output complementary filter tuning
float vel_Tau; // velocity state correction time constant (1/sec)
float pos_Tau; // postion state correction time constant (1/sec)
float vel_Tau{0.25f}; // velocity state correction time constant (1/sec)
float pos_Tau{0.25f}; // postion state correction time constant (1/sec)
// accel bias learning control
float acc_bias_lim; // maximum accel bias magnitude (m/s/s)
float acc_bias_learn_acc_lim; // learning is disabled if the magnitude of the IMU acceleration vector is greeater than this (m/sec**2)
float acc_bias_learn_gyr_lim; // learning is disabled if the magnitude of the IMU angular rate vector is greeater than this (rad/sec)
float acc_bias_learn_tc; // time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec)
float acc_bias_lim{0.4f}; // maximum accel bias magnitude (m/s/s)
float acc_bias_learn_acc_lim{25.0f}; // learning is disabled if the magnitude of the IMU acceleration vector is greeater than this (m/sec**2)
float acc_bias_learn_gyr_lim{3.0f}; // learning is disabled if the magnitude of the IMU angular rate vector is greeater than this (rad/sec)
float acc_bias_learn_tc{0.5f}; // time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec)
unsigned no_gps_timeout_max; // maximum time we allow dead reckoning while both gps position and velocity measurements are being
// rejected before attempting to reset the states to the GPS measurement (usec)
unsigned no_aid_timeout_max; // maximum lapsed time from last fusion of measurements that constrain drift before
// the EKF will report that it is dead-reckoning (usec)
unsigned no_gps_timeout_max{7000000}; // maximum time we allow dead reckoning while both gps position and velocity measurements are being
// rejected before attempting to reset the states to the GPS measurement (usec)
unsigned no_aid_timeout_max{1000000}; // maximum lapsed time from last fusion of measurements that constrain drift before
// the EKF will report that it is dead-reckoning (usec)
// multi-rotor drag specific force fusion
float drag_noise; // observation noise for drag specific force measurements (m/sec**2)
float bcoef_x; // ballistic coefficient along the X-axis (kg/m**2)
float bcoef_y; // ballistic coefficient along the Y-axis (kg/m**2)
// Initialize parameter values. Initialization must be accomplished in the constructor to allow C99 compiler compatibility.
parameters()
{
// measurement source control
fusion_mode = MASK_USE_GPS;
vdist_sensor_type = VDIST_SENSOR_BARO;
sensor_interval_min_ms = 20;
// measurement time delays
mag_delay_ms = 0.0f;
baro_delay_ms = 0.0f;
gps_delay_ms = 200.0f;
airspeed_delay_ms = 200.0f;
flow_delay_ms = 5.0f;
range_delay_ms = 5.0f;
ev_delay_ms = 100.0f;
// input noise
gyro_noise = 1.5e-2f;
accel_noise = 3.5e-1f;
// process noise
gyro_bias_p_noise = 1.0e-3f;
accel_bias_p_noise = 3.0e-3f;
mage_p_noise = 1.0e-3f;
magb_p_noise = 1.0e-4f;
wind_vel_p_noise = 1.0e-1f;
terrain_p_noise = 5.0f;
terrain_gradient = 0.5f;
// initialisation errors
switch_on_gyro_bias = 0.1f;
switch_on_accel_bias = 0.2f;
initial_tilt_err = 0.1f;
// position and velocity fusion
gps_vel_noise = 5.0e-1f;
gps_pos_noise = 0.5f;
pos_noaid_noise = 10.0f;
baro_noise = 2.0f;
baro_innov_gate = 5.0f;
posNE_innov_gate = 5.0f;
vel_innov_gate = 5.0f;
hgt_reset_lim = 0.0f;
// magnetometer fusion
mag_heading_noise = 3.0e-1f;
mag_noise = 5.0e-2f;
mag_declination_deg = 0.0f;
heading_innov_gate = 2.6f;
mag_innov_gate = 3.0f;
mag_declination_source = 7;
mag_fusion_type = 0;
// airspeed fusion
tas_innov_gate = 5.0f;
eas_noise = 1.4f;
// synthetic sideslip fusion
beta_innov_gate = 5.0f;
beta_noise = 0.3f;
beta_avg_ft_us = 1000000.0f; //1 Hz
// range finder fusion
range_noise = 0.1f;
range_innov_gate = 5.0f;
rng_gnd_clearance = 0.1f;
rng_sens_pitch = 0.0f;
range_noise_scaler = 0.0f;
// vision position fusion
ev_innov_gate = 5.0f;
// optical flow fusion
flow_noise = 0.15f;
flow_noise_qual_min = 0.5f;
flow_qual_min = 1;
flow_innov_gate = 3.0f;
flow_rate_max = 2.5f;
// GPS quality checks
gps_check_mask = 21;
req_hacc = 5.0f;
req_vacc = 8.0f;
req_sacc = 1.0f;
req_nsats = 6;
req_gdop = 2.0f;
req_hdrift = 0.3f;
req_vdrift = 0.5f;
// XYZ offset of sensors in body axes (m)
imu_pos_body = {};
gps_pos_body = {};
rng_pos_body = {};
flow_pos_body = {};
ev_pos_body = {};
// output complementary filter tuning time constants
vel_Tau = 0.25f;
pos_Tau = 0.25f;
// accel bias state limiting
acc_bias_lim = 0.4f;
acc_bias_learn_acc_lim = 25.0f;
acc_bias_learn_gyr_lim = 3.0f;
acc_bias_learn_tc = 0.5f;
// dead reckoning timers
no_gps_timeout_max = 7e6;
no_aid_timeout_max = 1e6;
// multi-rotor drag specific force fusion
drag_noise = 2.5f;
bcoef_x = 25.0f;
bcoef_y = 25.0f;
}
float drag_noise{2.5f}; // observation noise for drag specific force measurements (m/sec**2)
float bcoef_x{25.0f}; // ballistic coefficient along the X-axis (kg/m**2)
float bcoef_y{25.0f}; // ballistic coefficient along the Y-axis (kg/m**2)
};
struct stateSample {
@ -510,21 +392,21 @@ union filter_control_status_u { @@ -510,21 +392,21 @@ union filter_control_status_u {
};
union ekf_solution_status {
struct {
uint16_t attitude : 1; // 0 - True if the attitude estimate is good
uint16_t velocity_horiz : 1; // 1 - True if the horizontal velocity estimate is good
uint16_t velocity_vert : 1; // 2 - True if the vertical velocity estimate is good
uint16_t pos_horiz_rel : 1; // 3 - True if the horizontal position (relative) estimate is good
uint16_t pos_horiz_abs : 1; // 4 - True if the horizontal position (absolute) estimate is good
uint16_t pos_vert_abs : 1; // 5 - True if the vertical position (absolute) estimate is good
uint16_t pos_vert_agl : 1; // 6 - True if the vertical position (above ground) estimate is good
uint16_t const_pos_mode : 1; // 7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow)
uint16_t pred_pos_horiz_rel : 1; // 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate
uint16_t pred_pos_horiz_abs : 1; // 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate
uint16_t gps_glitch : 1; // 10 - True if the EKF has detected a GPS glitch
uint16_t accel_error : 1; // 11 - True if the EKF has detected bad accelerometer data
} flags;
uint16_t value;
struct {
uint16_t attitude : 1; // 0 - True if the attitude estimate is good
uint16_t velocity_horiz : 1; // 1 - True if the horizontal velocity estimate is good
uint16_t velocity_vert : 1; // 2 - True if the vertical velocity estimate is good
uint16_t pos_horiz_rel : 1; // 3 - True if the horizontal position (relative) estimate is good
uint16_t pos_horiz_abs : 1; // 4 - True if the horizontal position (absolute) estimate is good
uint16_t pos_vert_abs : 1; // 5 - True if the vertical position (absolute) estimate is good
uint16_t pos_vert_agl : 1; // 6 - True if the vertical position (above ground) estimate is good
uint16_t const_pos_mode : 1; // 7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow)
uint16_t pred_pos_horiz_rel : 1; // 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate
uint16_t pred_pos_horiz_abs : 1; // 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate
uint16_t gps_glitch : 1; // 10 - True if the EKF has detected a GPS glitch
uint16_t accel_error : 1; // 11 - True if the EKF has detected bad accelerometer data
} flags;
uint16_t value;
};
}

149
EKF/ekf.cpp
Unescape View File

@ -57,103 +57,6 @@ @@ -57,103 +57,6 @@
#define ISFINITE(x) __builtin_isfinite(x)
#endif
const float Ekf::_k_earth_rate = 0.000072921f;
const float Ekf::_gravity_mss = 9.80665f;
Ekf::Ekf():
_dt_update(0.01f),
_filter_initialised(false),
_earth_rate_initialised(false),
_fuse_height(false),
_fuse_pos(false),
_fuse_hor_vel(false),
_fuse_vert_vel(false),
_gps_data_ready(false),
_mag_data_ready(false),
_baro_data_ready(false),
_range_data_ready(false),
_flow_data_ready(false),
_ev_data_ready(false),
_tas_data_ready(false),
_time_last_fake_gps(0),
_time_last_pos_fuse(0),
_time_last_vel_fuse(0),
_time_last_hgt_fuse(0),
_time_last_of_fuse(0),
_time_last_arsp_fuse(0),
_time_last_beta_fuse(0),
_last_disarmed_posD(0.0f),
_imu_collection_time_adj(0.0f),
_time_acc_bias_check(0.0f),
_airspeed_innov(0.0f),
_airspeed_innov_var(0.0f),
_beta_innov(0.0f),
_beta_innov_var(0.0f),
_heading_innov(0.0f),
_heading_innov_var(0.0f),
_delta_time_of(0.0f),
_mag_declination(0.0f),
_gpsDriftVelN(0.0f),
_gpsDriftVelE(0.0f),
_gps_drift_velD(0.0f),
_gps_velD_diff_filt(0.0f),
_gps_velN_filt(0.0f),
_gps_velE_filt(0.0f),
_last_gps_fail_us(0),
_last_gps_origin_time_us(0),
_gps_alt_ref(0.0f),
_hgt_counter(0),
_rng_filt_state(0.0f),
_mag_counter(0),
_ev_counter(0),
_time_last_mag(0),
_hgt_sensor_offset(0.0f),
_baro_hgt_offset(0.0f),
_last_on_ground_posD(0.0f),
_terrain_vpos(0.0f),
_terrain_var(1.e4f),
_hagl_innov(0.0f),
_hagl_innov_var(0.0f),
_time_last_hagl_fuse(0),
_terrain_initialised(false),
_range_data_continuous(false),
_dt_last_range_update_filt_us(0.0f),
_baro_hgt_faulty(false),
_gps_hgt_faulty(false),
_rng_hgt_faulty(false),
_primary_hgt_source(VDIST_SENSOR_BARO),
_time_bad_vert_accel(0),
_time_good_vert_accel(0),
_bad_vert_accel_detected(false)
{
_state = {};
_last_known_posNE.setZero();
_earth_rate_NED.setZero();
_R_to_earth = matrix::Dcm<float>();
memset(_vel_pos_innov, 0, sizeof(_vel_pos_innov));
memset(_mag_innov, 0, sizeof(_mag_innov));
memset(_flow_innov, 0, sizeof(_flow_innov));
memset(_vel_pos_innov_var, 0, sizeof(_vel_pos_innov_var));
memset(_mag_innov_var, 0, sizeof(_mag_innov_var));
memset(_flow_innov_var, 0, sizeof(_flow_innov_var));
_delta_angle_corr.setZero();
_last_known_posNE.setZero();
_imu_down_sampled = {};
_q_down_sampled.setZero();
_vel_err_integ.setZero();
_pos_err_integ.setZero();
_mag_filt_state = {};
_delVel_sum = {};
_flow_gyro_bias = {};
_imu_del_ang_of = {};
_gps_check_fail_status.value = 0;
_state_reset_status = {};
_dt_ekf_avg = 0.001f * (float)(FILTER_UPDATE_PERIOD_MS);
memset(_drag_innov, 0, sizeof(_drag_innov));
memset(_drag_innov_var, 0, sizeof(_drag_innov_var));
}
bool Ekf::init(uint64_t timestamp)
{
bool ret = initialise_interface(timestamp);
@ -169,7 +72,7 @@ bool Ekf::init(uint64_t timestamp) @@ -169,7 +72,7 @@ bool Ekf::init(uint64_t timestamp)
_output_new.vel.setZero();
_output_new.pos.setZero();
_output_new.quat_nominal = matrix::Quaternion<float>();
_output_new.quat_nominal.setZero();
_delta_angle_corr.setZero();
_imu_down_sampled.delta_ang.setZero();
@ -209,7 +112,6 @@ bool Ekf::init(uint64_t timestamp) @@ -209,7 +112,6 @@ bool Ekf::init(uint64_t timestamp)
bool Ekf::update()
{
if (!_filter_initialised) {
_filter_initialised = initialiseFilter();
@ -258,13 +160,16 @@ bool Ekf::initialiseFilter() @@ -258,13 +160,16 @@ bool Ekf::initialiseFilter()
if ((_mag_counter == 0) && (_mag_sample_delayed.time_us != 0)) {
// initialise the counter when we start getting data from the buffer
_mag_counter = 1;
} else if ((_mag_counter != 0) && (_mag_sample_delayed.time_us != 0)) {
// increment the sample count and apply a LPF to the measurement
_mag_counter ++;
// don't start using data until we can be certain all bad initial data has been flushed
if (_mag_counter == (uint8_t)(_obs_buffer_length + 1)) {
// initialise filter states
_mag_filt_state = _mag_sample_delayed.mag;
} else if (_mag_counter > (uint8_t)(_obs_buffer_length + 1)) {
// noise filter the data
_mag_filt_state = _mag_filt_state * 0.9f + _mag_sample_delayed.mag * 0.1f;
@ -277,6 +182,7 @@ bool Ekf::initialiseFilter() @@ -277,6 +182,7 @@ bool Ekf::initialiseFilter()
if ((_ev_counter == 0) && (_ev_sample_delayed.time_us != 0)) {
// initialise the counter
_ev_counter = 1;
// set the height fusion mode to use external vision data when we start getting valid data from the buffer
if (_primary_hgt_source == VDIST_SENSOR_EV) {
_control_status.flags.baro_hgt = false;
@ -284,6 +190,7 @@ bool Ekf::initialiseFilter() @@ -284,6 +190,7 @@ bool Ekf::initialiseFilter()
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = true;
}
} else if ((_ev_counter != 0) && (_ev_sample_delayed.time_us != 0)) {
// increment the sample count
_ev_counter ++;
@ -305,13 +212,16 @@ bool Ekf::initialiseFilter() @@ -305,13 +212,16 @@ bool Ekf::initialiseFilter()
_control_status.flags.rng_hgt = true;
_control_status.flags.ev_hgt = false;
_hgt_counter = 1;
} else if ((_hgt_counter != 0) && (_range_sample_delayed.time_us != 0)) {
// increment the sample count and apply a LPF to the measurement
_hgt_counter ++;
// don't start using data until we can be certain all bad initial data has been flushed
if (_hgt_counter == (uint8_t)(_obs_buffer_length + 1)) {
// initialise filter states
_rng_filt_state = _range_sample_delayed.rng;
} else if (_hgt_counter > (uint8_t)(_obs_buffer_length + 1)) {
// noise filter the data
_rng_filt_state = 0.9f * _rng_filt_state + 0.1f * _range_sample_delayed.rng;
@ -329,13 +239,16 @@ bool Ekf::initialiseFilter() @@ -329,13 +239,16 @@ bool Ekf::initialiseFilter()
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_hgt_counter = 1;
} else if ((_hgt_counter != 0) && (_baro_sample_delayed.time_us != 0)) {
// increment the sample count and apply a LPF to the measurement
_hgt_counter ++;
// don't start using data until we can be certain all bad initial data has been flushed
if (_hgt_counter == (uint8_t)(_obs_buffer_length + 1)) {
// initialise filter states
_baro_hgt_offset = _baro_sample_delayed.hgt;
} else if (_hgt_counter > (uint8_t)(_obs_buffer_length + 1)) {
// noise filter the data
_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
@ -345,14 +258,16 @@ bool Ekf::initialiseFilter() @@ -345,14 +258,16 @@ bool Ekf::initialiseFilter()
} else if (_primary_hgt_source == VDIST_SENSOR_EV) {
_hgt_counter = _ev_counter;
} else {
return false;
}
// check to see if we have enough measurements and return false if not
bool hgt_count_fail = _hgt_counter <= 2*_obs_buffer_length;
bool mag_count_fail = _mag_counter <= 2*_obs_buffer_length;
bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= 2*_obs_buffer_length);
bool hgt_count_fail = _hgt_counter <= 2 * _obs_buffer_length;
bool mag_count_fail = _mag_counter <= 2 * _obs_buffer_length;
bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= 2 * _obs_buffer_length);
if (hgt_count_fail || mag_count_fail || ev_count_fail) {
return false;
@ -402,8 +317,9 @@ bool Ekf::initialiseFilter() @@ -402,8 +317,9 @@ bool Ekf::initialiseFilter()
// so it can be used as a backup ad set the initial height using the range finder
baroSample baro_newest = _baro_buffer.get_newest();
_baro_hgt_offset = baro_newest.hgt;
_state.pos(2) = -math::max(_rng_filt_state * _R_rng_to_earth_2_2,_params.rng_gnd_clearance);
_state.pos(2) = -math::max(_rng_filt_state * _R_rng_to_earth_2_2, _params.rng_gnd_clearance);
ECL_INFO("EKF using range finder height - commencing alignment");
} else if (_control_status.flags.ev_hgt) {
// if we are using external vision data for height, then the vertical position state needs to be reset
// because the initialisation position is not the zero datum
@ -475,10 +391,10 @@ void Ekf::predictState() @@ -475,10 +391,10 @@ void Ekf::predictState()
constrainStates();
// calculate an average filter update time
float input = 0.5f*(_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt);
float input = 0.5f * (_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt);
// filter and limit input between -50% and +100% of nominal value
input = math::constrain(input,0.0005f * (float)(FILTER_UPDATE_PERIOD_MS),0.002f * (float)(FILTER_UPDATE_PERIOD_MS));
input = math::constrain(input, 0.0005f * (float)(FILTER_UPDATE_PERIOD_MS), 0.002f * (float)(FILTER_UPDATE_PERIOD_MS));
_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input;
}
@ -515,6 +431,7 @@ bool Ekf::collect_imu(imuSample &imu) @@ -515,6 +431,7 @@ bool Ekf::collect_imu(imuSample &imu)
// if the target time delta between filter prediction steps has been exceeded
// write the accumulated IMU data to the ring buffer
float target_dt = (float)(FILTER_UPDATE_PERIOD_MS) / 1000;
if (_imu_down_sampled.delta_ang_dt >= target_dt - _imu_collection_time_adj) {
// accumulate the amount of time to advance the IMU collection time so that we meet the
@ -533,6 +450,7 @@ bool Ekf::collect_imu(imuSample &imu) @@ -533,6 +450,7 @@ bool Ekf::collect_imu(imuSample &imu)
_imu_down_sampled.delta_vel_dt = 0.0f;
_q_down_sampled(0) = 1.0f;
_q_down_sampled(1) = _q_down_sampled(2) = _q_down_sampled(3) = 0.0f;
return true;
}
@ -556,13 +474,13 @@ void Ekf::calculateOutputStates() @@ -556,13 +474,13 @@ void Ekf::calculateOutputStates()
// correct delta angles for bias offsets and scale factors
Vector3f delta_angle;
float dt_scale_correction = _dt_imu_avg/_dt_ekf_avg;
delta_angle(0) = _imu_sample_new.delta_ang(0) - _state.gyro_bias(0)*dt_scale_correction;
delta_angle(1) = _imu_sample_new.delta_ang(1) - _state.gyro_bias(1)*dt_scale_correction;
delta_angle(2) = _imu_sample_new.delta_ang(2) - _state.gyro_bias(2)*dt_scale_correction;
float dt_scale_correction = _dt_imu_avg / _dt_ekf_avg;
delta_angle(0) = _imu_sample_new.delta_ang(0) - _state.gyro_bias(0) * dt_scale_correction;
delta_angle(1) = _imu_sample_new.delta_ang(1) - _state.gyro_bias(1) * dt_scale_correction;
delta_angle(2) = _imu_sample_new.delta_ang(2) - _state.gyro_bias(2) * dt_scale_correction;
// correct delta velocity for bias offsets
Vector3f delta_vel = _imu_sample_new.delta_vel - _state.accel_bias*dt_scale_correction;
Vector3f delta_vel = _imu_sample_new.delta_vel - _state.accel_bias * dt_scale_correction;
// Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon
delta_angle += _delta_angle_corr;
@ -613,12 +531,14 @@ void Ekf::calculateOutputStates() @@ -613,12 +531,14 @@ void Ekf::calculateOutputStates()
// convert the quaternion delta to a delta angle
Vector3f delta_ang_error;
float scalar;
if (q_error(0) >= 0.0f) {
scalar = -2.0f;
} else {
scalar = 2.0f;
}
delta_ang_error(0) = scalar * q_error(1);
delta_ang_error(1) = scalar * q_error(2);
delta_ang_error(2) = scalar * q_error(3);
@ -656,9 +576,10 @@ void Ekf::calculateOutputStates() @@ -656,9 +576,10 @@ void Ekf::calculateOutputStates()
// this method is too expensive to use for the attitude states due to the quaternion operations required
// but does not introduce a time delay in the 'correction loop' and allows smaller tracking time constants
// to be used
outputSample output_states;
outputSample output_states = {};
unsigned max_index = _output_buffer.get_length() - 1;
for (unsigned index=0; index <= max_index; index++) {
for (unsigned index = 0; index <= max_index; index++) {
output_states = _output_buffer.get_from_index(index);
// a constant velocity correction is applied
@ -668,12 +589,10 @@ void Ekf::calculateOutputStates() @@ -668,12 +589,10 @@ void Ekf::calculateOutputStates()
output_states.pos += pos_correction;
// push the updated data to the buffer
_output_buffer.push_to_index(index,output_states);
_output_buffer.push_to_index(index, output_states);
}
// update output state to corrected values
_output_new = _output_buffer.get_newest();
}
}

198
EKF/ekf.h
Unescape View File

@ -47,7 +47,7 @@ class Ekf : public EstimatorInterface @@ -47,7 +47,7 @@ class Ekf : public EstimatorInterface
{
public:
Ekf();
Ekf() = default;
~Ekf() = default;
// initialise variables to sane values (also interface class)
@ -80,10 +80,10 @@ public: @@ -80,10 +80,10 @@ public:
void get_airspeed_innov_var(float *airspeed_innov_var);
// gets the innovations of synthetic sideslip measurement
void get_beta_innov(float *beta_innov);
void get_beta_innov(float *beta_innov);
// gets the innovation variance of the synthetic sideslip measurement
void get_beta_innov_var(float *beta_innov_var);
// gets the innovation variance of the synthetic sideslip measurement
void get_beta_innov_var(float *beta_innov_var);
// gets the innovation variance of the heading measurement
void get_heading_innov_var(float *heading_innov_var);
@ -165,7 +165,7 @@ public: @@ -165,7 +165,7 @@ public:
void get_gyro_bias(float bias[3]);
// get GPS check status
void get_gps_check_status(uint16_t *_gps_check_fail_status);
void get_gps_check_status(uint16_t *val);
// return the amount the local vertical position changed in the last reset and the number of reset events
void get_posD_reset(float *delta, uint8_t *counter) {*delta = _state_reset_status.posD_change; *counter = _state_reset_status.posD_counter;}
@ -174,13 +174,15 @@ public: @@ -174,13 +174,15 @@ public:
void get_velD_reset(float *delta, uint8_t *counter) {*delta = _state_reset_status.velD_change; *counter = _state_reset_status.velD_counter;}
// return the amount the local horizontal position changed in the last reset and the number of reset events
void get_posNE_reset(float delta[2], uint8_t *counter){
void get_posNE_reset(float delta[2], uint8_t *counter)
{
memcpy(delta, &_state_reset_status.posNE_change._data[0], sizeof(_state_reset_status.posNE_change._data));
*counter = _state_reset_status.posNE_counter;
}
// return the amount the local horizontal velocity changed in the last reset and the number of reset events
void get_velNE_reset(float delta[2], uint8_t *counter) {
void get_velNE_reset(float delta[2], uint8_t *counter)
{
memcpy(delta, &_state_reset_status.velNE_change._data[0], sizeof(_state_reset_status.velNE_change._data));
*counter = _state_reset_status.velNE_counter;
}
@ -204,9 +206,9 @@ public: @@ -204,9 +206,9 @@ public:
private:
static const uint8_t _k_num_states = 24;
static const float _k_earth_rate;
static const float _gravity_mss;
static constexpr uint8_t _k_num_states{24};
static constexpr float _k_earth_rate{0.000072921f};
static constexpr float _gravity_mss{9.80665f};
// reset event monitoring
// structure containing velocity, position, height and yaw reset information
@ -221,145 +223,145 @@ private: @@ -221,145 +223,145 @@ private:
Vector2f posNE_change; // North, East position change due to last reset (m)
float posD_change; // Down position change due to last reset (m)
Quaternion quat_change; // quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion
} _state_reset_status;
} _state_reset_status{};
float _dt_ekf_avg; // average update rate of the ekf
float _dt_update; // delta time since last ekf update. This time can be used for filters
// which run at the same rate as the Ekf::update() function
float _dt_ekf_avg{0.001f * FILTER_UPDATE_PERIOD_MS}; // average update rate of the ekf
float _dt_update{0.01f}; // delta time since last ekf update. This time can be used for filters
// which run at the same rate as the Ekf::update() function
stateSample _state; // state struct of the ekf running at the delayed time horizon
stateSample _state{}; // state struct of the ekf running at the delayed time horizon
bool _filter_initialised; // true when the EKF sttes and covariances been initialised
bool _earth_rate_initialised; // true when we know the earth rotatin rate (requires GPS)
bool _filter_initialised{false}; // true when the EKF sttes and covariances been initialised
bool _earth_rate_initialised{false}; // true when we know the earth rotatin rate (requires GPS)
bool _fuse_height; // baro height data should be fused
bool _fuse_pos; // gps position data should be fused
bool _fuse_hor_vel; // gps horizontal velocity measurement should be fused
bool _fuse_vert_vel; // gps vertical velocity measurement should be fused
bool _fuse_height{false}; // baro height data should be fused
bool _fuse_pos{false}; // gps position data should be fused
bool _fuse_hor_vel{false}; // gps horizontal velocity measurement should be fused
bool _fuse_vert_vel{false}; // gps vertical velocity measurement should be fused
// booleans true when fresh sensor data is available at the fusion time horizon
bool _gps_data_ready;
bool _mag_data_ready;
bool _baro_data_ready;
bool _range_data_ready;
bool _flow_data_ready;
bool _ev_data_ready;
bool _tas_data_ready;
uint64_t _time_last_fake_gps; // last time in us at which we have faked gps measurement for static mode
uint64_t _time_last_pos_fuse; // time the last fusion of horizontal position measurements was performed (usec)
uint64_t _time_last_vel_fuse; // time the last fusion of velocity measurements was performed (usec)
uint64_t _time_last_hgt_fuse; // time the last fusion of height measurements was performed (usec)
uint64_t _time_last_of_fuse; // time the last fusion of optical flow measurements were performed (usec)
uint64_t _time_last_arsp_fuse; // time the last fusion of airspeed measurements were performed (usec)
uint64_t _time_last_beta_fuse; // time the last fusion of synthetic sideslip measurements were performed (usec)
bool _gps_data_ready{false};
bool _mag_data_ready{false};
bool _baro_data_ready{false};
bool _range_data_ready{false};
bool _flow_data_ready{false};
bool _ev_data_ready{false};
bool _tas_data_ready{false};
uint64_t _time_last_fake_gps{0}; // last time in us at which we have faked gps measurement for static mode
uint64_t _time_last_pos_fuse{0}; // time the last fusion of horizontal position measurements was performed (usec)
uint64_t _time_last_vel_fuse{0}; // time the last fusion of velocity measurements was performed (usec)
uint64_t _time_last_hgt_fuse{0}; // time the last fusion of height measurements was performed (usec)
uint64_t _time_last_of_fuse{0}; // time the last fusion of optical flow measurements were performed (usec)
uint64_t _time_last_arsp_fuse{0}; // time the last fusion of airspeed measurements were performed (usec)
uint64_t _time_last_beta_fuse{0}; // time the last fusion of synthetic sideslip measurements were performed (usec)
Vector2f _last_known_posNE; // last known local NE position vector (m)
float _last_disarmed_posD; // vertical position recorded at arming (m)
float _imu_collection_time_adj; // the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
float _last_disarmed_posD{0.0f}; // vertical position recorded at arming (m)
float _imu_collection_time_adj{0.0f}; // the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
uint64_t _time_acc_bias_check; // last time the accel bias check passed (usec)
uint64_t _time_acc_bias_check{0}; // last time the accel bias check passed (usec)
Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s
matrix::Dcm<float> _R_to_earth; // transformation matrix from body frame to earth frame from last EKF predition
float P[_k_num_states][_k_num_states]{}; // state covariance matrix
float P[_k_num_states][_k_num_states] {}; // state covariance matrix
float _vel_pos_innov[6]{}; // innovations: 0-2 vel, 3-5 pos
float _vel_pos_innov_var[6]{}; // innovation variances: 0-2 vel, 3-5 pos
float _vel_pos_innov[6] {}; // innovations: 0-2 vel, 3-5 pos
float _vel_pos_innov_var[6] {}; // innovation variances: 0-2 vel, 3-5 pos
float _mag_innov[3]{}; // earth magnetic field innovations
float _mag_innov_var[3]{}; // earth magnetic field innovation variance
float _mag_innov[3] {}; // earth magnetic field innovations
float _mag_innov_var[3] {}; // earth magnetic field innovation variance
float _airspeed_innov; // airspeed measurement innovation
float _airspeed_innov_var; // airspeed measurement innovation variance
float _airspeed_innov{0.0f}; // airspeed measurement innovation
float _airspeed_innov_var{0.0f}; // airspeed measurement innovation variance
float _beta_innov; // synthetic sideslip measurement innovation
float _beta_innov_var; // synthetic sideslip measurement innovation variance
float _beta_innov{0.0f}; // synthetic sideslip measurement innovation
float _beta_innov_var{0.0f}; // synthetic sideslip measurement innovation variance
float _drag_innov[2]; // multirotor drag measurement innovation
float _drag_innov_var[2]; // multirotor drag measurement innovation variance
float _drag_innov[2] {}; // multirotor drag measurement innovation
float _drag_innov_var[2] {}; // multirotor drag measurement innovation variance
float _heading_innov; // heading measurement innovation
float _heading_innov_var; // heading measurement innovation variance
float _heading_innov{0.0f}; // heading measurement innovation
float _heading_innov_var{0.0f}; // heading measurement innovation variance
// optical flow processing
float _flow_innov[2]{}; // flow measurement innovation
float _flow_innov_var[2]{}; // flow innovation variance
float _flow_innov[2] {}; // flow measurement innovation
float _flow_innov_var[2] {}; // flow innovation variance
Vector3f _flow_gyro_bias; // bias errors in optical flow sensor rate gyro outputs
Vector3f _imu_del_ang_of; // bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates
float _delta_time_of; // time in sec that _imu_del_ang_of was accumulated over
float _delta_time_of{0.0f}; // time in sec that _imu_del_ang_of was accumulated over
float _mag_declination; // magnetic declination used by reset and fusion functions (rad)
float _mag_declination{0.0f}; // magnetic declination used by reset and fusion functions (rad)
// output predictor states
Vector3f _delta_angle_corr; // delta angle correction vector
imuSample _imu_down_sampled; // down sampled imu data (sensor rate -> filter update rate)
imuSample _imu_down_sampled{}; // down sampled imu data (sensor rate -> filter update rate)
Quaternion _q_down_sampled; // down sampled quaternion (tracking delta angles between ekf update steps)
Vector3f _vel_err_integ; // integral of velocity tracking error
Vector3f _pos_err_integ; // integral of position tracking error
float _output_tracking_error[3]{};// contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
float _output_tracking_error[3] {}; // contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
// variables used for the GPS quality checks
float _gpsDriftVelN; // GPS north position derivative (m/s)
float _gpsDriftVelE; // GPS east position derivative (m/s)
float _gps_drift_velD; // GPS down position derivative (m/s)
float _gps_velD_diff_filt; // GPS filtered Down velocity (m/s)
float _gps_velN_filt; // GPS filtered North velocity (m/s)
float _gps_velE_filt; // GPS filtered East velocity (m/s)
uint64_t _last_gps_fail_us; // last system time in usec that the GPS failed it's checks
float _gpsDriftVelN{0.0f}; // GPS north position derivative (m/s)
float _gpsDriftVelE{0.0f}; // GPS east position derivative (m/s)
float _gps_drift_velD{0.0f}; // GPS down position derivative (m/s)
float _gps_velD_diff_filt{0.0f}; // GPS filtered Down velocity (m/s)
float _gps_velN_filt{0.0f}; // GPS filtered North velocity (m/s)
float _gps_velE_filt{0.0f}; // GPS filtered East velocity (m/s)
uint64_t _last_gps_fail_us{0}; // last system time in usec that the GPS failed it's checks
// Variables used to publish the WGS-84 location of the EKF local NED origin
uint64_t _last_gps_origin_time_us; // time the origin was last set (uSec)
float _gps_alt_ref; // WGS-84 height (m)
uint64_t _last_gps_origin_time_us{0}; // time the origin was last set (uSec)
float _gps_alt_ref{0.0f}; // WGS-84 height (m)
// Variables used to initialise the filter states
uint32_t _hgt_counter; // number of height samples read during initialisation
float _rng_filt_state; // filtered height measurement
uint32_t _mag_counter; // number of magnetometer samples read during initialisation
uint32_t _ev_counter; // number of exgernal vision samples read during initialisation
uint64_t _time_last_mag; // measurement time of last magnetomter sample
uint32_t _hgt_counter{0}; // number of height samples read during initialisation
float _rng_filt_state{0.0f}; // filtered height measurement
uint32_t _mag_counter{0}; // number of magnetometer samples read during initialisation
uint32_t _ev_counter{0}; // number of exgernal vision samples read during initialisation
uint64_t _time_last_mag{0}; // measurement time of last magnetomter sample
Vector3f _mag_filt_state; // filtered magnetometer measurement
Vector3f _delVel_sum; // summed delta velocity
float _hgt_sensor_offset; // set as necessary if desired to maintain the same height after a height reset (m)
float _baro_hgt_offset; // baro height reading at the local NED origin (m)
float _hgt_sensor_offset{0.0f}; // set as necessary if desired to maintain the same height after a height reset (m)
float _baro_hgt_offset{0.0f}; // baro height reading at the local NED origin (m)
// Variables used to control activation of post takeoff functionality
float _last_on_ground_posD; // last vertical position when the in_air status was false (m)
float _last_on_ground_posD{0.0f}; // last vertical position when the in_air status was false (m)
gps_check_fail_status_u _gps_check_fail_status{};
// variables used to inhibit accel bias learning
bool _accel_bias_inhibit; // true when the accel bias learning is being inhibited
float _accel_mag_filt; // acceleration magnitude after application of a decaying envelope filter (m/sec**2)
float _ang_rate_mag_filt; // angular rate magnitude after application of a decaying envelope filter (rad/sec)
bool _accel_bias_inhibit{false}; // true when the accel bias learning is being inhibited
float _accel_mag_filt{0.0f}; // acceleration magnitude after application of a decaying envelope filter (m/sec**2)
float _ang_rate_mag_filt{0.0f}; // angular rate magnitude after application of a decaying envelope filter (rad/sec)
Vector3f _prev_dvel_bias_var; // saved delta velocity XYZ bias variances (m/sec)**2
// Terrain height state estimation
float _terrain_vpos; // estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
float _terrain_var; // variance of terrain position estimate (m^2)
float _hagl_innov; // innovation of the last height above terrain measurement (m)
float _hagl_innov_var; // innovation variance for the last height above terrain measurement (m^2)
float _terrain_vpos{0.0f}; // estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
float _terrain_var{1e4f}; // variance of terrain position estimate (m^2)
float _hagl_innov{0.0f}; // innovation of the last height above terrain measurement (m)
float _hagl_innov_var{0.0f}; // innovation variance for the last height above terrain measurement (m^2)
uint64_t _time_last_hagl_fuse; // last system time in usec that the hagl measurement failed it's checks
bool _terrain_initialised; // true when the terrain estimator has been intialised
float _sin_tilt_rng; // sine of the range finder tilt rotation about the Y body axis
float _cos_tilt_rng; // cosine of the range finder tilt rotation about the Y body axis
float _R_rng_to_earth_2_2; // 2,2 element of the rotation matrix from sensor frame to earth frame
bool _range_data_continuous; // true when we are receiving range finder data faster than a 2Hz average
float _dt_last_range_update_filt_us; // filtered value of the delta time elapsed since the last range measurement came into
// the filter (microseconds)
bool _terrain_initialised{false}; // true when the terrain estimator has been intialised
float _sin_tilt_rng{0.0f}; // sine of the range finder tilt rotation about the Y body axis
float _cos_tilt_rng{0.0f}; // cosine of the range finder tilt rotation about the Y body axis
float _R_rng_to_earth_2_2{0.0f}; // 2,2 element of the rotation matrix from sensor frame to earth frame
bool _range_data_continuous{false}; // true when we are receiving range finder data faster than a 2Hz average
float _dt_last_range_update_filt_us{0.0f}; // filtered value of the delta time elapsed since the last range measurement came into
// the filter (microseconds)
// height sensor fault status
bool _baro_hgt_faulty; // true if valid baro data is unavailable for use
bool _gps_hgt_faulty; // true if valid gps height data is unavailable for use
bool _rng_hgt_faulty; // true if valid rnage finder height data is unavailable for use
int _primary_hgt_source; // priary source of height data set at initialisation
bool _baro_hgt_faulty{false}; // true if valid baro data is unavailable for use
bool _gps_hgt_faulty{false}; // true if valid gps height data is unavailable for use
bool _rng_hgt_faulty{false}; // true if valid rnage finder height data is unavailable for use
int _primary_hgt_source{VDIST_SENSOR_BARO}; // priary source of height data set at initialisation
// imu fault status
uint64_t _time_bad_vert_accel; // last time a bad vertical accel was detected (usec)
uint64_t _time_good_vert_accel; // last time a good vertical accel was detected (usec)
bool _bad_vert_accel_detected; // true when bad vertical accelerometer data has been detected
uint64_t _time_bad_vert_accel{0}; // last time a bad vertical accel was detected (usec)
uint64_t _time_good_vert_accel{0}; // last time a good vertical accel was detected (usec)
bool _bad_vert_accel_detected{false}; // true when bad vertical accelerometer data has been detected
// update the real time complementary filter states. This includes the prediction
// and the correction step
@ -390,7 +392,7 @@ private: @@ -390,7 +392,7 @@ private:
void fuseAirspeed();
// fuse synthetic zero sideslip measurement
void fuseSideslip();
void fuseSideslip();
// fuse body frame drag specific forces for multi-rotor wind estimation
void fuseDrag();

131
EKF/ekf_helper.cpp
Unescape View File

@ -60,17 +60,17 @@ bool Ekf::resetVelocity() @@ -60,17 +60,17 @@ bool Ekf::resetVelocity()
} else {
return false;
}
// calculate the change in velocity and apply to the output predictor state history
Vector3f velocity_change = _state.vel - vel_before_reset;
outputSample output_states;
outputSample output_states = {};
unsigned max_index = _output_buffer.get_length() - 1;
for (unsigned index=0; index <= max_index; index++) {
for (unsigned index = 0; index <= max_index; index++) {
output_states = _output_buffer.get_from_index(index);
output_states.vel += velocity_change;
_output_buffer.push_to_index(index,output_states);
_output_buffer.push_to_index(index, output_states);
}
@ -119,13 +119,14 @@ bool Ekf::resetPosition() @@ -119,13 +119,14 @@ bool Ekf::resetPosition()
Vector2f posNE_change;
posNE_change(0) = _state.pos(0) - posNE_before_reset(0);
posNE_change(1) = _state.pos(1) - posNE_before_reset(1);
outputSample output_states;
outputSample output_states = {};
unsigned max_index = _output_buffer.get_length() - 1;
for (unsigned index=0; index <= max_index; index++) {
for (unsigned index = 0; index <= max_index; index++) {
output_states = _output_buffer.get_from_index(index);
output_states.pos(0) += posNE_change(0);
output_states.pos(1) += posNE_change(1);
_output_buffer.push_to_index(index,output_states);
_output_buffer.push_to_index(index, output_states);
}
// apply the change in position to our newest position estimate
@ -230,8 +231,10 @@ void Ekf::resetHeight() @@ -230,8 +231,10 @@ void Ekf::resetHeight()
// use the most recent data if it's time offset from the fusion time horizon is smaller
int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us;
int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us;
if (std::abs(dt_newest) < std::abs(dt_delayed)) {
_state.pos(2) = ev_newest.posNED(2);
} else {
_state.pos(2) = _ev_sample_delayed.posNED(2);
}
@ -259,6 +262,7 @@ void Ekf::resetHeight() @@ -259,6 +262,7 @@ void Ekf::resetHeight()
P[6][6] = 10.0f;
}
vert_vel_reset = true;
// store the reset amount and time to be published
@ -283,9 +287,10 @@ void Ekf::resetHeight() @@ -283,9 +287,10 @@ void Ekf::resetHeight()
}
// add the reset amount to the output observer buffered data
outputSample output_states;
outputSample output_states = {};
unsigned output_length = _output_buffer.get_length();
for (unsigned i=0; i < output_length; i++) {
for (unsigned i = 0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
if (vert_pos_reset) {
@ -296,7 +301,7 @@ void Ekf::resetHeight() @@ -296,7 +301,7 @@ void Ekf::resetHeight()
output_states.vel(2) += _state_reset_status.velD_change;
}
_output_buffer.push_to_index(i,output_states);
_output_buffer.push_to_index(i, output_states);
}
}
@ -314,15 +319,16 @@ void Ekf::alignOutputFilter() @@ -314,15 +319,16 @@ void Ekf::alignOutputFilter()
Vector3f pos_delta = _state.pos - _output_sample_delayed.pos;
// loop through the output filter state history and add the deltas
outputSample output_states;
outputSample output_states = {};
unsigned output_length = _output_buffer.get_length();
for (unsigned i=0; i < output_length; i++) {
for (unsigned i = 0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal *= q_delta;
output_states.quat_nominal.normalize();
output_states.vel += vel_delta;
output_states.pos += pos_delta;
_output_buffer.push_to_index(i,output_states);
_output_buffer.push_to_index(i, output_states);
}
}
@ -356,12 +362,14 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) @@ -356,12 +362,14 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
// convert the observed quaternion to a rotation matrix
matrix::Dcm<float> R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
// calculate the yaw angle for a 312 sequence
euler321(2) = atan2f(R_to_earth_ev(1, 0) , R_to_earth_ev(0, 0));
euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
// the angle of the projection onto the horizontal gives the yaw angle
euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
} else {
// there is no yaw observation
return false;
@ -377,9 +385,9 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) @@ -377,9 +385,9 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
// Calculate the 312 sequence euler angles that rotate from earth to body frame
// See http://www.atacolorado.com/eulersequences.doc
Vector3f euler312;
euler312(0) = atan2f(-_R_to_earth(0, 1) , _R_to_earth(1, 1)); // first rotation (yaw)
euler312(0) = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)); // first rotation (yaw)
euler312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
euler312(2) = atan2f(-_R_to_earth(2, 0) , _R_to_earth(2, 2)); // third rotation (pitch)
euler312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch)
// Set the first rotation (yaw) to zero and calculate the rotation matrix from body to earth frame
euler312(0) = 0.0f;
@ -408,12 +416,14 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) @@ -408,12 +416,14 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
// convert the observed quaternion to a rotation matrix
matrix::Dcm<float> R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
// calculate the yaw angle for a 312 sequence
euler312(0) = atan2f(-R_to_earth_ev(0, 1) , R_to_earth_ev(1, 1));
euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
// the angle of the projection onto the horizontal gives the yaw angle
euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
} else {
// there is no yaw observation
return false;
@ -444,6 +454,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) @@ -444,6 +454,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
if (_params.fusion_mode & MASK_USE_EVYAW) {
// using error estimate from external vision data
angle_err_var_vec(2) = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// using magnetic heading tuning parameter
angle_err_var_vec(2) = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
@ -467,12 +478,13 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) @@ -467,12 +478,13 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
// add the reset amount to the output observer buffered data
outputSample output_states;
outputSample output_states = {};
unsigned output_length = _output_buffer.get_length();
for (unsigned i=0; i < output_length; i++) {
for (unsigned i = 0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal *= _state_reset_status.quat_change;
_output_buffer.push_to_index(i,output_states);
_output_buffer.push_to_index(i, output_states);
}
// apply the change in attitude quaternion to our newest quaternion estimate
@ -578,13 +590,13 @@ void Ekf::get_mag_innov(float mag_innov[3]) @@ -578,13 +590,13 @@ void Ekf::get_mag_innov(float mag_innov[3])
// gets the innovations of the airspeed measnurement
void Ekf::get_airspeed_innov(float *airspeed_innov)
{
memcpy(airspeed_innov,&_airspeed_innov, sizeof(float));
memcpy(airspeed_innov, &_airspeed_innov, sizeof(float));
}
// gets the innovations of the synthetic sideslip measurements
void Ekf::get_beta_innov(float *beta_innov)
{
memcpy(beta_innov,&_beta_innov, sizeof(float));
memcpy(beta_innov, &_beta_innov, sizeof(float));
}
// gets the innovations of the heading measurement
@ -670,9 +682,9 @@ void Ekf::get_state_delayed(float *state) @@ -670,9 +682,9 @@ void Ekf::get_state_delayed(float *state)
void Ekf::get_accel_bias(float bias[3])
{
float temp[3];
temp[0] = _state.accel_bias(0) /_dt_ekf_avg;
temp[1] = _state.accel_bias(1) /_dt_ekf_avg;
temp[2] = _state.accel_bias(2) /_dt_ekf_avg;
temp[0] = _state.accel_bias(0) / _dt_ekf_avg;
temp[1] = _state.accel_bias(1) / _dt_ekf_avg;
temp[2] = _state.accel_bias(2) / _dt_ekf_avg;
memcpy(bias, temp, 3 * sizeof(float));
}
@ -680,9 +692,9 @@ void Ekf::get_accel_bias(float bias[3]) @@ -680,9 +692,9 @@ void Ekf::get_accel_bias(float bias[3])
void Ekf::get_gyro_bias(float bias[3])
{
float temp[3];
temp[0] = _state.gyro_bias(0) /_dt_ekf_avg;
temp[1] = _state.gyro_bias(1) /_dt_ekf_avg;
temp[2] = _state.gyro_bias(2) /_dt_ekf_avg;
temp[0] = _state.gyro_bias(0) / _dt_ekf_avg;
temp[1] = _state.gyro_bias(1) / _dt_ekf_avg;
temp[2] = _state.gyro_bias(2) / _dt_ekf_avg;
memcpy(bias, temp, 3 * sizeof(float));
}
@ -731,6 +743,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko @@ -731,6 +743,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko
float hpos_err;
float vpos_err;
bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos);
if (vel_pos_aiding && _NED_origin_initialised) {
hpos_err = sqrtf(P[7][7] + P[8][8] + sq(_gps_origin_eph));
vpos_err = sqrtf(P[9][9] + sq(_gps_origin_epv));
@ -745,7 +758,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko @@ -745,7 +758,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko
// The reason is that complete rejection of measurements is often casued by heading misalignment or inertial sensing errors
// and using state variances for accuracy reporting is overly optimistic in these situations
if (_is_dead_reckoning && (_control_status.flags.gps || _control_status.flags.ev_pos)) {
hpos_err = math::max(hpos_err, sqrtf(_vel_pos_innov[3]*_vel_pos_innov[3] + _vel_pos_innov[4]*_vel_pos_innov[4]));
hpos_err = math::max(hpos_err, sqrtf(_vel_pos_innov[3] * _vel_pos_innov[3] + _vel_pos_innov[4] * _vel_pos_innov[4]));
}
@ -761,6 +774,7 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko @@ -761,6 +774,7 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko
float hpos_err;
float vpos_err;
bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos);
if (vel_pos_aiding && _NED_origin_initialised) {
hpos_err = sqrtf(P[7][7] + P[8][8]);
vpos_err = sqrtf(P[9][9]);
@ -775,7 +789,7 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko @@ -775,7 +789,7 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko
// The reason is that complete rejection of measurements is often casued by heading misalignment or inertial sensing errors
// and using state variances for accuracy reporting is overly optimistic in these situations
if (_is_dead_reckoning && (_control_status.flags.gps || _control_status.flags.ev_pos)) {
hpos_err = math::max(hpos_err, sqrtf(_vel_pos_innov[3]*_vel_pos_innov[3] + _vel_pos_innov[4]*_vel_pos_innov[4]));
hpos_err = math::max(hpos_err, sqrtf(_vel_pos_innov[3] * _vel_pos_innov[3] + _vel_pos_innov[4] * _vel_pos_innov[4]));
}
@ -790,6 +804,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckon @@ -790,6 +804,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckon
float hvel_err;
float vvel_err;
bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos);
if (vel_pos_aiding && _NED_origin_initialised) {
hvel_err = sqrtf(P[4][4] + P[5][5]);
vvel_err = sqrtf(P[6][6]);
@ -804,14 +819,19 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckon @@ -804,14 +819,19 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckon
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
// and using state variances for accuracy reporting is overly optimistic in these situations
float vel_err_conservative = 0.0f;
if (_is_dead_reckoning) {
if (_control_status.flags.opt_flow) {
float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f);
vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * sqrtf(_flow_innov[0]*_flow_innov[0] + _flow_innov[1]*_flow_innov[1]);
vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)),
gndclearance) * sqrtf(_flow_innov[0] * _flow_innov[0] + _flow_innov[1] * _flow_innov[1]);
}
if (_control_status.flags.gps || _control_status.flags.ev_pos) {
vel_err_conservative = math::max(vel_err_conservative, sqrtf(_vel_pos_innov[0]*_vel_pos_innov[0] + _vel_pos_innov[1]*_vel_pos_innov[1]));
vel_err_conservative = math::max(vel_err_conservative,
sqrtf(_vel_pos_innov[0] * _vel_pos_innov[0] + _vel_pos_innov[1] * _vel_pos_innov[1]));
}
hvel_err = math::max(hvel_err, vel_err_conservative);
}
@ -830,11 +850,11 @@ void Ekf::get_innovation_test_status(uint16_t *status, float *mag, float *vel, f @@ -830,11 +850,11 @@ void Ekf::get_innovation_test_status(uint16_t *status, float *mag, float *vel, f
// return the integer bitmask containing the consistency check pass/fail satus
*status = _innov_check_fail_status.value;
// return the largest magnetometer innovation test ratio
*mag = sqrtf(math::max(_yaw_test_ratio,math::max(math::max(_mag_test_ratio[0],_mag_test_ratio[1]),_mag_test_ratio[2])));
*mag = sqrtf(math::max(_yaw_test_ratio, math::max(math::max(_mag_test_ratio[0], _mag_test_ratio[1]), _mag_test_ratio[2])));
// return the largest NED velocity innovation test ratio
*vel = sqrtf(math::max(math::max(_vel_pos_test_ratio[0],_vel_pos_test_ratio[1]),_vel_pos_test_ratio[2]));
*vel = sqrtf(math::max(math::max(_vel_pos_test_ratio[0], _vel_pos_test_ratio[1]), _vel_pos_test_ratio[2]));
// return the largest NE position innovation test ratio
*pos = sqrtf(math::max(_vel_pos_test_ratio[3],_vel_pos_test_ratio[4]));
*pos = sqrtf(math::max(_vel_pos_test_ratio[3], _vel_pos_test_ratio[4]));
// return the vertical position innovation test ratio
*hgt = sqrtf(_vel_pos_test_ratio[5]);
// return the airspeed fusion innovation test ratio
@ -872,6 +892,7 @@ void Ekf::fuse(float *K, float innovation) @@ -872,6 +892,7 @@ void Ekf::fuse(float *K, float innovation)
for (unsigned i = 0; i < 4; i++) {
_state.quat_nominal(i) = _state.quat_nominal(i) - K[i] * innovation;
}
_state.quat_nominal.normalize();
for (unsigned i = 0; i < 3; i++) {
@ -933,7 +954,8 @@ bool Ekf::global_position_is_valid() @@ -933,7 +954,8 @@ bool Ekf::global_position_is_valid()
// return true if we are totally reliant on inertial dead-reckoning for position
bool Ekf::inertial_dead_reckoning()
{
bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos) && ((_time_last_imu - _time_last_pos_fuse <= 1E6) || (_time_last_imu - _time_last_vel_fuse <= 1E6));
bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos)
&& ((_time_last_imu - _time_last_pos_fuse <= 1E6) || (_time_last_imu - _time_last_vel_fuse <= 1E6));
bool optFlowAiding = _control_status.flags.opt_flow && (_time_last_imu - _time_last_of_fuse <= 1E6);
bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= 1E6);
@ -944,14 +966,14 @@ bool Ekf::inertial_dead_reckoning() @@ -944,14 +966,14 @@ bool Ekf::inertial_dead_reckoning()
Vector3f EstimatorInterface::cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2)
{
Vector3f vecOut;
vecOut(0) = vecIn1(1)*vecIn2(2) - vecIn1(2)*vecIn2(1);
vecOut(1) = vecIn1(2)*vecIn2(0) - vecIn1(0)*vecIn2(2);
vecOut(2) = vecIn1(0)*vecIn2(1) - vecIn1(1)*vecIn2(0);
vecOut(0) = vecIn1(1) * vecIn2(2) - vecIn1(2) * vecIn2(1);
vecOut(1) = vecIn1(2) * vecIn2(0) - vecIn1(0) * vecIn2(2);
vecOut(2) = vecIn1(0) * vecIn2(1) - vecIn1(1) * vecIn2(0);
return vecOut;
}
// calculate the inverse rotation matrix from a quaternion rotation
Matrix3f EstimatorInterface::quat_to_invrotmat(const Quaternion& quat)
Matrix3f EstimatorInterface::quat_to_invrotmat(const Quaternion &quat)
{
float q00 = quat(0) * quat(0);
float q11 = quat(1) * quat(1);
@ -965,15 +987,15 @@ Matrix3f EstimatorInterface::quat_to_invrotmat(const Quaternion& quat) @@ -965,15 +987,15 @@ Matrix3f EstimatorInterface::quat_to_invrotmat(const Quaternion& quat)
float q23 = quat(2) * quat(3);
Matrix3f dcm;
dcm(0,0) = q00 + q11 - q22 - q33;
dcm(1,1) = q00 - q11 + q22 - q33;
dcm(2,2) = q00 - q11 - q22 + q33;
dcm(0,1) = 2.0f * (q12 - q03);
dcm(0,2) = 2.0f * (q13 + q02);
dcm(1,0) = 2.0f * (q12 + q03);
dcm(1,2) = 2.0f * (q23 - q01);
dcm(2,0) = 2.0f * (q13 - q02);
dcm(2,1) = 2.0f * (q23 + q01);
dcm(0, 0) = q00 + q11 - q22 - q33;
dcm(1, 1) = q00 - q11 + q22 - q33;
dcm(2, 2) = q00 - q11 - q22 + q33;
dcm(0, 1) = 2.0f * (q12 - q03);
dcm(0, 2) = 2.0f * (q13 + q02);
dcm(1, 0) = 2.0f * (q12 + q03);
dcm(1, 2) = 2.0f * (q23 - q01);
dcm(2, 0) = 2.0f * (q13 - q02);
dcm(2, 1) = 2.0f * (q23 + q01);
return dcm;
}
@ -982,12 +1004,14 @@ Matrix3f EstimatorInterface::quat_to_invrotmat(const Quaternion& quat) @@ -982,12 +1004,14 @@ Matrix3f EstimatorInterface::quat_to_invrotmat(const Quaternion& quat)
Vector3f Ekf::calcRotVecVariances()
{
Vector3f rot_var_vec;
float q0,q1,q2,q3;
float q0, q1, q2, q3;
if (_state.quat_nominal(0) >= 0.0f) {
q0 = _state.quat_nominal(0);
q1 = _state.quat_nominal(1);
q2 = _state.quat_nominal(2);
q3 = _state.quat_nominal(3);
} else {
q0 = -_state.quat_nominal(0);
q1 = -_state.quat_nominal(1);
@ -1033,6 +1057,7 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var) @@ -1033,6 +1057,7 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
q1 = _state.quat_nominal(1);
q2 = _state.quat_nominal(2);
q3 = _state.quat_nominal(3);
} else {
q0 = -_state.quat_nominal(0);
q1 = -_state.quat_nominal(1);
@ -1120,17 +1145,17 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var) @@ -1120,17 +1145,17 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
P[0][2] = 0.0f;
P[0][3] = 0.0f;
P[1][0] = 0.0f;
P[1][1] = 0.25f*rot_vec_var(0);
P[1][1] = 0.25f * rot_vec_var(0);
P[1][2] = 0.0f;
P[1][3] = 0.0f;
P[2][0] = 0.0f;
P[2][1] = 0.0f;
P[2][2] = 0.25f*rot_vec_var(1);
P[2][2] = 0.25f * rot_vec_var(1);
P[2][3] = 0.0f;
P[3][0] = 0.0f;
P[3][1] = 0.0f;
P[3][2] = 0.0f;
P[3][3] = 0.25f*rot_vec_var(2);
P[3][3] = 0.25f * rot_vec_var(2);
}
}

82
EKF/estimator_interface.cpp
Unescape View File

@ -40,67 +40,15 @@ @@ -40,67 +40,15 @@
* @author Siddharth B Purohit <siddharthbharatpurohit@gmail.com>
*/
#include <inttypes.h>
#include <math.h>
#include "../ecl.h"
#include "estimator_interface.h"
#include "mathlib.h"
EstimatorInterface::EstimatorInterface():
_obs_buffer_length(10),
_imu_buffer_length(30),
_min_obs_interval_us(0),
_dt_imu_avg(0.0f),
_mag_sample_delayed{},
_baro_sample_delayed{},
_gps_sample_delayed{},
_range_sample_delayed{},
_airspeed_sample_delayed{},
_flow_sample_delayed{},
_ev_sample_delayed{},
_drag_sample_delayed{},
_drag_down_sampled{},
_drag_sample_count(0),
_drag_sample_time_dt(0.0f),
_air_density(1.225f),
_imu_ticks(0),
_imu_updated(false),
_initialised(false),
_NED_origin_initialised(false),
_gps_speed_valid(false),
_gps_origin_eph(0.0f),
_gps_origin_epv(0.0f),
_pos_ref{},
_gps_pos_prev{},
_gps_alt_prev(0.0f),
_yaw_test_ratio(0.0f),
_mag_test_ratio{},
_vel_pos_test_ratio{},
_tas_test_ratio(0.0f),
_terr_test_ratio(0.0f),
_beta_test_ratio(0.0f),
_drag_test_ratio{},
_is_dead_reckoning(false),
_vibe_metrics{},
_time_last_imu(0),
_time_last_gps(0),
_time_last_mag(0),
_time_last_baro(0),
_time_last_range(0),
_time_last_airspeed(0),
_time_last_ext_vision(0),
_time_last_optflow(0),
_mag_declination_gps(0.0f),
_mag_declination_to_save_deg(0.0f)
{
_delta_ang_prev.setZero();
_delta_vel_prev.setZero();
}
#include "../ecl.h"
#include <math.h>
#include "mathlib.h"
// Accumulate imu data and store to buffer at desired rate
void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float (&delta_ang)[3],
float (&delta_vel)[3])
void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt,
float (&delta_ang)[3], float (&delta_vel)[3])
{
if (!_initialised) {
init(time_usec);
@ -129,7 +77,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u @@ -129,7 +77,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
_imu_ticks++;
// calculate a metric which indicates the amount of coning vibration
Vector3f temp = cross_product(imu_sample_new.delta_ang , _delta_ang_prev);
Vector3f temp = cross_product(imu_sample_new.delta_ang, _delta_ang_prev);
_vibe_metrics[0] = 0.99f * _vibe_metrics[0] + 0.01f * temp.norm();
// calculate a metric which indiates the amount of high frequency gyro vibration
@ -160,6 +108,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u @@ -160,6 +108,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
// calculate the downsample ratio for drag specific force data
uint8_t min_sample_ratio = (uint8_t) ceilf((float)_imu_buffer_length / _obs_buffer_length);
if (min_sample_ratio < 5) {
min_sample_ratio = 5;
@ -189,7 +138,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u @@ -189,7 +138,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
// calculate the minimum interval between observations required to guarantee no loss of data
// this will occur if data is overwritten before its time stamp falls behind the fusion time horizon
_min_obs_interval_us = (_imu_sample_new.time_us - _imu_sample_delayed.time_us)/(_obs_buffer_length - 1);
_min_obs_interval_us = (_imu_sample_new.time_us - _imu_sample_delayed.time_us) / (_obs_buffer_length - 1);
} else {
_imu_updated = false;
@ -203,7 +152,7 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3]) @@ -203,7 +152,7 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3])
if (time_usec - _time_last_mag > _min_obs_interval_us) {
magSample mag_sample_new = {};
mag_sample_new.time_us = time_usec - _params.mag_delay_ms * 1000;
mag_sample_new.time_us = time_usec - _params.mag_delay_ms * 1000;
mag_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
_time_last_mag = time_usec;
@ -223,6 +172,7 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps) @@ -223,6 +172,7 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
// limit data rate to prevent data being lost
bool need_gps = (_params.fusion_mode & MASK_USE_GPS) || (_params.vdist_sensor_type == VDIST_SENSOR_GPS);
if (((time_usec - _time_last_gps) > _min_obs_interval_us) && need_gps && gps->fix_type > 2) {
gpsSample gps_sample_new = {};
gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000;
@ -353,6 +303,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl @@ -353,6 +303,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate is produced by a RH rotation of the image about the sensor axis.
// copy the optical and gyro measured delta angles
optflow_sample_new.gyroXYZ = - flow->gyrodata;
if (flow_quality_good) {
optflow_sample_new.flowRadXY = - flow->flowdata;
@ -362,6 +313,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl @@ -362,6 +313,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
optflow_sample_new.flowRadXY(1) = + flow->gyrodata(1);
}
// compensate for body motion to give a LOS rate
optflow_sample_new.flowRadXYcomp(0) = optflow_sample_new.flowRadXY(0) - optflow_sample_new.gyroXYZ(0);
optflow_sample_new.flowRadXYcomp(1) = optflow_sample_new.flowRadXY(1) - optflow_sample_new.gyroXYZ(1);
@ -418,7 +370,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) @@ -418,7 +370,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
_obs_buffer_length = (ekf_delay_ms / _params.sensor_interval_min_ms) + 1;
// limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)
_obs_buffer_length = math::min(_obs_buffer_length,_imu_buffer_length);
_obs_buffer_length = math::min(_obs_buffer_length, _imu_buffer_length);
if (!(_imu_buffer.allocate(_imu_buffer_length) &&
_gps_buffer.allocate(_obs_buffer_length) &&
@ -436,7 +388,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) @@ -436,7 +388,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
}
// zero the data in the observation buffers
for (int index=0; index < _obs_buffer_length; index++) {
for (int index = 0; index < _obs_buffer_length; index++) {
gpsSample gps_sample_init = {};
_gps_buffer.push(gps_sample_init);
magSample mag_sample_init = {};
@ -456,7 +408,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) @@ -456,7 +408,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
}
// zero the data in the imu data and output observer state buffers
for (int index=0; index < _imu_buffer_length; index++) {
for (int index = 0; index < _imu_buffer_length; index++) {
imuSample imu_sample_init = {};
_imu_buffer.push(imu_sample_init);
outputSample output_sample_init = {};
@ -505,6 +457,6 @@ bool EstimatorInterface::local_position_is_valid() @@ -505,6 +457,6 @@ bool EstimatorInterface::local_position_is_valid()
{
// return true if the position estimate is valid
return (((_time_last_imu - _time_last_optflow) < 5e6) && _control_status.flags.opt_flow) ||
(((_time_last_imu - _time_last_ext_vision) < 5e6) && _control_status.flags.ev_pos) ||
global_position_is_valid();
(((_time_last_imu - _time_last_ext_vision) < 5e6) && _control_status.flags.ev_pos) ||
global_position_is_valid();
}

115
EKF/estimator_interface.h
Unescape View File

@ -39,7 +39,6 @@ @@ -39,7 +39,6 @@
*
*/
#include <stdint.h>
#include <matrix/matrix/math.hpp>
#include "RingBuffer.h"
#include "geo.h"
@ -47,11 +46,12 @@ @@ -47,11 +46,12 @@
#include "mathlib.h"
using namespace estimator;
class EstimatorInterface
{
public:
EstimatorInterface();
EstimatorInterface() = default;
~EstimatorInterface() = default;
virtual bool init(uint64_t timestamp) = 0;
@ -65,7 +65,7 @@ public: @@ -65,7 +65,7 @@ public:
virtual void get_mag_innov(float mag_innov[3]) = 0;
// gets the innovation of airspeed measurement
virtual void get_airspeed_innov(float *airspeed_innov) = 0;
virtual void get_airspeed_innov(float *airspeed_innov) = 0;
// gets the innovation of the synthetic sideslip measurement
virtual void get_beta_innov(float *beta_innov) = 0;
@ -81,7 +81,7 @@ public: @@ -81,7 +81,7 @@ public:
virtual void get_mag_innov_var(float mag_innov_var[3]) = 0;
// gets the innovation variance of the airspeed measurement
virtual void get_airspeed_innov_var(float *get_airspeed_innov_var) = 0;
virtual void get_airspeed_innov_var(float *get_airspeed_innov_var) = 0;
// gets the innovation variance of the synthetic sideslip measurement
virtual void get_beta_innov_var(float *get_beta_innov_var) = 0;
@ -211,13 +211,14 @@ public: @@ -211,13 +211,14 @@ public:
void get_velocity(float *vel)
{
// calculate the average angular rate across the last IMU update
Vector3f ang_rate = _imu_sample_new.delta_ang * (1.0f/_imu_sample_new.delta_ang_dt);
Vector3f ang_rate = _imu_sample_new.delta_ang * (1.0f / _imu_sample_new.delta_ang_dt);
// calculate the velocity of the relative to the body origin
// Note % operator has been overloaded to performa cross product
Vector3f vel_imu_rel_body = cross_product(ang_rate , _params.imu_pos_body);
Vector3f vel_imu_rel_body = cross_product(ang_rate, _params.imu_pos_body);
// rotate the relative velocty into earth frame and subtract from the EKF velocity
// (which is at the IMU) to get velocity of the body origin
Vector3f vel_earth = _output_new.vel - _R_to_earth_now * vel_imu_rel_body;
// copy to output
for (unsigned i = 0; i < 3; i++) {
vel[i] = vel_earth(i);
@ -228,6 +229,7 @@ public: @@ -228,6 +229,7 @@ public:
{
// rotate the position of the IMU relative to the boy origin into earth frame
Vector3f pos_offset_earth = _R_to_earth_now * _params.imu_pos_body;
// subtract from the EKF position (which is at the IMU) to get position at the body origin
for (unsigned i = 0; i < 3; i++) {
pos[i] = _output_new.pos(i) - pos_offset_earth(i);
@ -298,72 +300,73 @@ protected: @@ -298,72 +300,73 @@ protected:
max freq (Hz) = (OBS_BUFFER_LENGTH - 1) / (IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS * 0.001)
This can be adjusted to match the max sensor data rate plus some margin for jitter.
*/
uint8_t _obs_buffer_length;
uint8_t _obs_buffer_length{0};
/*
IMU_BUFFER_LENGTH defines how many IMU samples we buffer which sets the time delay from current time to the
EKF fusion time horizon and therefore the maximum sensor time offset relative to the IMU that we can compensate for.
max sensor time offet (msec) = IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS
This can be adjusted to a value that is FILTER_UPDATE_PERIOD_MS longer than the maximum observation time delay.
*/
uint8_t _imu_buffer_length;
uint8_t _imu_buffer_length{0};
static const unsigned FILTER_UPDATE_PERIOD_MS = 12; // ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta
unsigned _min_obs_interval_us; // minimum time interval between observations that will guarantee data is not lost (usec)
unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)
float _dt_imu_avg; // average imu update period in s
float _dt_imu_avg{0.0f}; // average imu update period in s
imuSample _imu_sample_delayed; // captures the imu sample on the delayed time horizon
imuSample _imu_sample_delayed{}; // captures the imu sample on the delayed time horizon
// measurement samples capturing measurements on the delayed time horizon
magSample _mag_sample_delayed;
baroSample _baro_sample_delayed;
gpsSample _gps_sample_delayed;
rangeSample _range_sample_delayed;
airspeedSample _airspeed_sample_delayed;
flowSample _flow_sample_delayed;
extVisionSample _ev_sample_delayed;
dragSample _drag_sample_delayed;
dragSample _drag_down_sampled; // down sampled drag specific force data (filter prediction rate -> observation rate)
magSample _mag_sample_delayed{};
baroSample _baro_sample_delayed{};
gpsSample _gps_sample_delayed{};
rangeSample _range_sample_delayed{};
airspeedSample _airspeed_sample_delayed{};
flowSample _flow_sample_delayed{};
extVisionSample _ev_sample_delayed{};
dragSample _drag_sample_delayed{};
dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate)
// Used by the multi-rotor specific drag force fusion
uint8_t _drag_sample_count; // number of drag specific force samples assumulated at the filter prediction rate
float _drag_sample_time_dt; // time integral across all samples used to form _drag_down_sampled (sec)
float _air_density; // air density (kg/m**3)
uint8_t _drag_sample_count{0}; // number of drag specific force samples assumulated at the filter prediction rate
float _drag_sample_time_dt{0.0f}; // time integral across all samples used to form _drag_down_sampled (sec)
float _air_density{1.225f}; // air density (kg/m**3)
outputSample _output_sample_delayed; // filter output on the delayed time horizon
outputSample _output_new; // filter output on the non-delayed time horizon
imuSample _imu_sample_new; // imu sample capturing the newest imu data
outputSample _output_sample_delayed{}; // filter output on the delayed time horizon
outputSample _output_new{}; // filter output on the non-delayed time horizon
imuSample _imu_sample_new{}; // imu sample capturing the newest imu data
Matrix3f _R_to_earth_now; // rotation matrix from body to earth frame at current time
uint64_t _imu_ticks; // counter for imu updates
uint64_t _imu_ticks{0}; // counter for imu updates
bool _imu_updated; // true if the ekf should update (completed downsampling process)
bool _initialised; // true if the ekf interface instance (data buffering) is initialized
bool _imu_updated{false}; // true if the ekf should update (completed downsampling process)
bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized
bool _NED_origin_initialised;
bool _gps_speed_valid;
float _gps_origin_eph; // horizontal position uncertainty of the GPS origin
float _gps_origin_epv; // vertical position uncertainty of the GPS origin
struct map_projection_reference_s _pos_ref; // Contains WGS-84 position latitude and longitude (radians) of the EKF origin
struct map_projection_reference_s _gps_pos_prev; // Contains WGS-84 position latitude and longitude (radians) of the previous GPS message
float _gps_alt_prev; // height from the previous GPS message (m)
bool _NED_origin_initialised{false};
bool _gps_speed_valid{false};
float _gps_origin_eph{0.0f}; // horizontal position uncertainty of the GPS origin
float _gps_origin_epv{0.0f}; // vertical position uncertainty of the GPS origin
struct map_projection_reference_s _pos_ref {}; // Contains WGS-84 position latitude and longitude (radians) of the EKF origin
struct map_projection_reference_s _gps_pos_prev {}; // Contains WGS-84 position latitude and longitude (radians) of the previous GPS message
float _gps_alt_prev{0.0f}; // height from the previous GPS message (m)
// innovation consistency check monitoring ratios
float _yaw_test_ratio; // yaw innovation consistency check ratio
float _mag_test_ratio[3]; // magnetometer XYZ innovation consistency check ratios
float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios
float _tas_test_ratio; // tas innovation consistency check ratio
float _terr_test_ratio; // height above terrain measurement innovation consistency check ratio
float _beta_test_ratio; // sideslip innovation consistency check ratio
float _drag_test_ratio[2]; // drag innovation cinsistency check ratio
float _yaw_test_ratio{0.0f}; // yaw innovation consistency check ratio
float _mag_test_ratio[3] {}; // magnetometer XYZ innovation consistency check ratios
float _vel_pos_test_ratio[6] {}; // velocity and position innovation consistency check ratios
float _tas_test_ratio{0.0f}; // tas innovation consistency check ratio
float _terr_test_ratio{0.0f}; // height above terrain measurement innovation consistency check ratio
float _beta_test_ratio{0.0f}; // sideslip innovation consistency check ratio
float _drag_test_ratio[2] {}; // drag innovation cinsistency check ratio
innovation_fault_status_u _innov_check_fail_status{};
bool _is_dead_reckoning; // true if we are no longer fusing measurements that constrain horizontal velocity drift
bool _is_dead_reckoning{false}; // true if we are no longer fusing measurements that constrain horizontal velocity drift
// IMU vibration monitoring
Vector3f _delta_ang_prev; // delta angle from the previous IMU measurement
Vector3f _delta_vel_prev; // delta velocity from the previous IMU measurement
float _vibe_metrics[3]; // IMU vibration metrics
float _vibe_metrics[3] {}; // IMU vibration metrics
// [0] Level of coning vibration in the IMU delta angles (rad^2)
// [1] high frequency vibraton level in the IMU delta angle data (rad)
// [2] high frequency vibration level in the IMU delta velocity data (m/s)
@ -380,14 +383,14 @@ protected: @@ -380,14 +383,14 @@ protected:
RingBuffer<outputSample> _output_buffer;
RingBuffer<dragSample> _drag_buffer;
uint64_t _time_last_imu; // timestamp of last imu sample in microseconds
uint64_t _time_last_gps; // timestamp of last gps measurement in microseconds
uint64_t _time_last_mag; // timestamp of last magnetometer measurement in microseconds
uint64_t _time_last_baro; // timestamp of last barometer measurement in microseconds
uint64_t _time_last_range; // timestamp of last range measurement in microseconds
uint64_t _time_last_airspeed; // timestamp of last airspeed measurement in microseconds
uint64_t _time_last_ext_vision; // timestamp of last external vision measurement in microseconds
uint64_t _time_last_optflow;
uint64_t _time_last_imu{0}; // timestamp of last imu sample in microseconds
uint64_t _time_last_gps{0}; // timestamp of last gps measurement in microseconds
uint64_t _time_last_mag{0}; // timestamp of last magnetometer measurement in microseconds
uint64_t _time_last_baro{0}; // timestamp of last barometer measurement in microseconds
uint64_t _time_last_range{0}; // timestamp of last range measurement in microseconds
uint64_t _time_last_airspeed{0}; // timestamp of last airspeed measurement in microseconds
uint64_t _time_last_ext_vision{0}; // timestamp of last external vision measurement in microseconds
uint64_t _time_last_optflow{0};
fault_status_u _fault_status{};
@ -397,8 +400,8 @@ protected: @@ -397,8 +400,8 @@ protected:
// free buffer memory
void unallocate_buffers();
float _mag_declination_gps; // magnetic declination returned by the geo library using the last valid GPS position (rad)
float _mag_declination_to_save_deg; // magnetic declination to save to EKF2_MAG_DECL (deg)
float _mag_declination_gps{0.0f}; // magnetic declination returned by the geo library using the last valid GPS position (rad)
float _mag_declination_to_save_deg{0.0f}; // magnetic declination to save to EKF2_MAG_DECL (deg)
// this is the current status of the filter control modes
filter_control_status_u _control_status{};
@ -410,6 +413,6 @@ protected: @@ -410,6 +413,6 @@ protected:
Vector3f cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2);
// calculate the inverse rotation matrix from a quaternion rotation
Matrix3f quat_to_invrotmat(const Quaternion& quat);
Matrix3f quat_to_invrotmat(const Quaternion &quat);
};

Write Preview
Loading…
Cancel
Save