mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-16 12:17:35 +08:00
Moved initialization to object constructors to allow C99 compiler compatibility.
This commit is contained in:
mcsauder
+126
-77
@@ -31,14 +31,17 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "Matrix.hpp"
|
||||
|
||||
/**
|
||||
* @file estimator_base.h
|
||||
* @file common.h
|
||||
* Definition of base class for attitude estimators
|
||||
*
|
||||
* @author Roman Bast <bapstroman@gmail.com>
|
||||
* @author Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
|
||||
*
|
||||
*/
|
||||
|
||||
namespace estimator
|
||||
{
|
||||
struct gps_message {
|
||||
@@ -64,45 +67,45 @@ typedef matrix::Quaternion<float> Quaternion;
|
||||
typedef matrix::Matrix<float, 3, 3> Matrix3f;
|
||||
|
||||
struct outputSample {
|
||||
Quaternion quat_nominal; // nominal quaternion describing vehicle attitude
|
||||
Vector3f vel; // NED velocity estimate in earth frame in m/s
|
||||
Vector3f pos; // NED position estimate in earth frame in m/s
|
||||
uint64_t time_us; // timestamp in microseconds
|
||||
Quaternion quat_nominal; // nominal quaternion describing vehicle attitude
|
||||
Vector3f vel; // NED velocity estimate in earth frame in m/s
|
||||
Vector3f pos; // NED position estimate in earth frame in m/s
|
||||
uint64_t time_us; // timestamp in microseconds
|
||||
};
|
||||
|
||||
struct imuSample {
|
||||
Vector3f delta_ang; // delta angle in body frame (integrated gyro measurements)
|
||||
Vector3f delta_vel; // delta velocity in body frame (integrated accelerometer measurements)
|
||||
float delta_ang_dt; // delta angle integration period in seconds
|
||||
float delta_vel_dt; // delta velocity integration period in seconds
|
||||
uint64_t time_us; // timestamp in microseconds
|
||||
Vector3f delta_ang; // delta angle in body frame (integrated gyro measurements)
|
||||
Vector3f delta_vel; // delta velocity in body frame (integrated accelerometer measurements)
|
||||
float delta_ang_dt; // delta angle integration period in seconds
|
||||
float delta_vel_dt; // delta velocity integration period in seconds
|
||||
uint64_t time_us; // timestamp in microseconds
|
||||
};
|
||||
|
||||
struct gpsSample {
|
||||
Vector2f pos; // NE earth frame gps horizontal position measurement in m
|
||||
float hgt; // gps height measurement in m
|
||||
Vector3f vel; // NED earth frame gps velocity measurement in m/s
|
||||
uint64_t time_us; // timestamp in microseconds
|
||||
Vector2f pos; // NE earth frame gps horizontal position measurement in m
|
||||
float hgt; // gps height measurement in m
|
||||
Vector3f vel; // NED earth frame gps velocity measurement in m/s
|
||||
uint64_t time_us; // timestamp in microseconds
|
||||
};
|
||||
|
||||
struct magSample {
|
||||
Vector3f mag; // NED magnetometer body frame measurements
|
||||
uint64_t time_us; // timestamp in microseconds
|
||||
Vector3f mag; // NED magnetometer body frame measurements
|
||||
uint64_t time_us; // timestamp in microseconds
|
||||
};
|
||||
|
||||
struct baroSample {
|
||||
float hgt; // barometer height above sea level measurement in m
|
||||
uint64_t time_us; // timestamp in microseconds
|
||||
float hgt; // barometer height above sea level measurement in m
|
||||
uint64_t time_us; // timestamp in microseconds
|
||||
};
|
||||
|
||||
struct rangeSample {
|
||||
float rng; // range (distance to ground) measurement in m
|
||||
uint64_t time_us; // timestamp in microseconds
|
||||
float rng; // range (distance to ground) measurement in m
|
||||
uint64_t time_us; // timestamp in microseconds
|
||||
};
|
||||
|
||||
struct airspeedSample {
|
||||
float airspeed; // airspeed measurement in m/s
|
||||
uint64_t time_us; // timestamp in microseconds
|
||||
float airspeed; // airspeed measurement in m/s
|
||||
uint64_t time_us; // timestamp in microseconds
|
||||
};
|
||||
|
||||
struct flowSample {
|
||||
@@ -112,83 +115,129 @@ struct flowSample {
|
||||
};
|
||||
|
||||
struct parameters {
|
||||
float mag_delay_ms = 0.0f; // magnetometer measurement delay relative to the IMU
|
||||
float baro_delay_ms = 0.0f; // barometer height measurement delay relative to the IMU
|
||||
float gps_delay_ms = 200.0f; // GPS measurement delay relative to the IMU
|
||||
float airspeed_delay_ms = 200.0f; // airspeed measurement delay relative to the IMU
|
||||
float mag_delay_ms; // magnetometer measurement delay relative to the IMU
|
||||
float baro_delay_ms; // barometer height measurement delay relative to the IMU
|
||||
float gps_delay_ms; // GPS measurement delay relative to the IMU
|
||||
float airspeed_delay_ms; // airspeed measurement delay relative to the IMU
|
||||
|
||||
// input noise
|
||||
float gyro_noise = 1.0e-3f; // IMU angular rate noise used for covariance prediction
|
||||
float accel_noise = 2.5e-1f; // IMU acceleration noise use for covariance prediction
|
||||
float gyro_noise; // IMU angular rate noise used for covariance prediction
|
||||
float accel_noise; // IMU acceleration noise use for covariance prediction
|
||||
|
||||
// process noise
|
||||
float gyro_bias_p_noise = 7.0e-5f; // process noise for IMU delta angle bias prediction
|
||||
float accel_bias_p_noise = 1.0e-4f; // process noise for IMU delta velocity bias prediction
|
||||
float gyro_scale_p_noise = 3.0e-3f; // process noise for gyro scale factor prediction
|
||||
float mag_p_noise = 2.5e-2f; // process noise for magnetic field prediction
|
||||
float wind_vel_p_noise = 1.0e-1f; // process noise for wind velocity prediction
|
||||
float gyro_bias_p_noise; // process noise for IMU delta angle bias prediction
|
||||
float accel_bias_p_noise; // process noise for IMU delta velocity bias prediction
|
||||
float gyro_scale_p_noise; // process noise for gyro scale factor prediction
|
||||
float mag_p_noise; // process noise for magnetic field prediction
|
||||
float wind_vel_p_noise; // process noise for wind velocity prediction
|
||||
|
||||
float gps_vel_noise = 5.0e-1f; // observation noise for gps velocity fusion
|
||||
float gps_pos_noise = 1.0f; // observation noise for gps position fusion
|
||||
float pos_noaid_noise = 10.0f; // observation noise for non-aiding position fusion
|
||||
float baro_noise = 3.0f; // observation noise for barometric height fusion
|
||||
float baro_innov_gate = 3.0f; // barometric height innovation consistency gate size in standard deviations
|
||||
float posNE_innov_gate = 3.0f; // GPS horizontal position innovation consistency gate size in standard deviations
|
||||
float vel_innov_gate = 3.0f; // GPS velocity innovation consistency gate size in standard deviations
|
||||
float gps_vel_noise; // observation noise for gps velocity fusion
|
||||
float gps_pos_noise; // observation noise for gps position fusion
|
||||
float pos_noaid_noise; // observation noise for non-aiding position fusion
|
||||
float baro_noise; // observation noise for barometric height fusion
|
||||
float baro_innov_gate; // barometric height innovation consistency gate size in standard deviations
|
||||
float posNE_innov_gate; // GPS horizontal position innovation consistency gate size in standard deviations
|
||||
float vel_innov_gate; // GPS velocity innovation consistency gate size in standard deviations
|
||||
|
||||
float mag_heading_noise = 1.7e-1f; // measurement noise used for simple heading fusion
|
||||
float mag_noise = 5.0e-2f; // measurement noise used for 3-axis magnetoemeter fusion
|
||||
float mag_declination_deg = 0.0f; // magnetic declination in degrees
|
||||
float heading_innov_gate = 3.0f; // heading fusion innovation consistency gate size in standard deviations
|
||||
float mag_innov_gate = 3.0f; // magnetometer fusion innovation consistency gate size in standard deviations
|
||||
int mag_declination_source = 3; // bitmask used to control the handling of declination data
|
||||
int mag_fusion_type = 0; // integer used to specify the type of magnetometer fusion used
|
||||
float mag_heading_noise; // measurement noise used for simple heading fusion
|
||||
float mag_noise; // measurement noise used for 3-axis magnetoemeter fusion
|
||||
float mag_declination_deg; // magnetic declination in degrees
|
||||
float heading_innov_gate; // heading fusion innovation consistency gate size in standard deviations
|
||||
float mag_innov_gate; // magnetometer fusion innovation consistency gate size in standard deviations
|
||||
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
|
||||
|
||||
// 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 = 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
|
||||
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
|
||||
|
||||
// Initialize parameter values. Initialization must be accomplished in the constructor to allow C99 compiler compatibility.
|
||||
parameters()
|
||||
{
|
||||
mag_delay_ms = 0.0f;
|
||||
baro_delay_ms = 0.0f;
|
||||
gps_delay_ms = 200.0f;
|
||||
airspeed_delay_ms = 200.0f;
|
||||
|
||||
// input noise
|
||||
gyro_noise = 1.0e-3f;
|
||||
accel_noise = 2.5e-1f;
|
||||
|
||||
// process noise
|
||||
gyro_bias_p_noise = 7.0e-5f;
|
||||
accel_bias_p_noise = 1.0e-4f;
|
||||
gyro_scale_p_noise = 3.0e-3f;
|
||||
mag_p_noise = 2.5e-2f;
|
||||
wind_vel_p_noise = 1.0e-1f;
|
||||
|
||||
gps_vel_noise = 5.0e-1f;
|
||||
gps_pos_noise = 1.0f;
|
||||
pos_noaid_noise = 10.0f;
|
||||
baro_noise = 3.0f;
|
||||
baro_innov_gate = 3.0f;
|
||||
posNE_innov_gate = 3.0f;
|
||||
vel_innov_gate = 3.0f;
|
||||
|
||||
mag_heading_noise = 1.7e-1f;
|
||||
mag_noise = 5.0e-2f;
|
||||
mag_declination_deg = 0.0f;
|
||||
heading_innov_gate = 3.0f;
|
||||
mag_innov_gate = 3.0f;
|
||||
|
||||
mag_declination_source = 7;
|
||||
mag_fusion_type = 0;
|
||||
|
||||
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;
|
||||
}
|
||||
};
|
||||
|
||||
// Bit locations for mag_declination_source
|
||||
#define MASK_USE_GEO_DECL (1<<0) // set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value
|
||||
#define MASK_SAVE_GEO_DECL (1<<1) // set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library
|
||||
#define MASK_FUSE_DECL (1<<2) // set to true if the declination is always fused as an observation to contrain drift when 3-axis fusion is performed
|
||||
#define MASK_FUSE_DECL (1<<2) // set to true if the declination is always fused as an observation to constrain drift when 3-axis fusion is performed
|
||||
|
||||
// Integer definitions for mag_fusion_type
|
||||
#define MAG_FUSE_TYPE_AUTO 0 // The selection of either heading or 3D magnetometer fusion will be automatic
|
||||
#define MAG_FUSE_TYPE_HEADING 1 // Magnetic heading fusion will alays be used. This is less accurate, but less affected by earth field distortions
|
||||
#define MAG_FUSE_TYPE_3D 2 // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
|
||||
#define MAG_FUSE_TYPE_HEADING 1 // Magnetic heading fusion will always be used. This is less accurate, but less affected by earth field distortions
|
||||
#define MAG_FUSE_TYPE_3D 2 // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localized earth field distortions
|
||||
|
||||
struct stateSample {
|
||||
Vector3f ang_error; // attitude axis angle error (error state formulation)
|
||||
Vector3f vel; // NED velocity in earth frame in m/s
|
||||
Vector3f pos; // NED position in earth frame in m
|
||||
Vector3f gyro_bias; // gyro bias estimate in rad/s
|
||||
Vector3f gyro_scale; // gyro scale estimate
|
||||
float accel_z_bias; // accelerometer z axis bias estimate
|
||||
Vector3f mag_I; // NED earth magnetic field in gauss
|
||||
Vector3f mag_B; // magnetometer bias estimate in body frame in gauss
|
||||
Vector2f wind_vel; // wind velocity in m/s
|
||||
Quaternion quat_nominal; // nominal quaternion describing vehicle attitude
|
||||
Vector3f ang_error; // attitude axis angle error (error state formulation)
|
||||
Vector3f vel; // NED velocity in earth frame in m/s
|
||||
Vector3f pos; // NED position in earth frame in m
|
||||
Vector3f gyro_bias; // gyro bias estimate in rad/s
|
||||
Vector3f gyro_scale; // gyro scale estimate
|
||||
float accel_z_bias; // accelerometer z axis bias estimate
|
||||
Vector3f mag_I; // NED earth magnetic field in gauss
|
||||
Vector3f mag_B; // magnetometer bias estimate in body frame in gauss
|
||||
Vector2f wind_vel; // wind velocity in m/s
|
||||
Quaternion quat_nominal; // nominal quaternion describing vehicle attitude
|
||||
};
|
||||
|
||||
struct fault_status_t {
|
||||
bool bad_mag_x: 1; // true if the fusion of the magnetometer X-axis has encountered a numerical error
|
||||
bool bad_mag_y: 1; // true if the fusion of the magnetometer Y-axis has encountered a numerical error
|
||||
bool bad_mag_z: 1; // true if the fusion of the magnetometer Z-axis has encountered a numerical error
|
||||
bool bad_mag_hdg: 1; // true if the fusion of the magnetic heading has encountered a numerical error
|
||||
bool bad_mag_decl: 1; // true if the fusion of the magnetic declination has encountered a numerical error
|
||||
bool bad_airspeed: 1; // true if fusion of the airspeed has encountered a numerical error
|
||||
bool bad_sideslip: 1; // true if fusion of the synthetic sideslip constraint has encountered a numerical error
|
||||
bool bad_optflow_X: 1; // true if fusion of the optical flow X axis has encountered a numerical error
|
||||
bool bad_optflow_Y: 1; // true if fusion of the optical flow Y axis has encountered a numerical error
|
||||
bool bad_mag_x: 1; // true if the fusion of the magnetometer X-axis has encountered a numerical error
|
||||
bool bad_mag_y: 1; // true if the fusion of the magnetometer Y-axis has encountered a numerical error
|
||||
bool bad_mag_z: 1; // true if the fusion of the magnetometer Z-axis has encountered a numerical error
|
||||
bool bad_mag_hdg: 1; // true if the fusion of the magnetic heading has encountered a numerical error
|
||||
bool bad_mag_decl: 1; // true if the fusion of the magnetic declination has encountered a numerical error
|
||||
bool bad_airspeed: 1; // true if fusion of the airspeed has encountered a numerical error
|
||||
bool bad_sideslip: 1; // true if fusion of the synthetic sideslip constraint has encountered a numerical error
|
||||
bool bad_optflow_X: 1; // true if fusion of the optical flow X axis has encountered a numerical error
|
||||
bool bad_optflow_Y: 1; // true if fusion of the optical flow Y axis has encountered a numerical error
|
||||
};
|
||||
|
||||
// publish the status of various GPS quality checks
|
||||
|
||||
+58
-47
@@ -42,39 +42,58 @@
|
||||
#include "ekf.h"
|
||||
|
||||
Ekf::Ekf():
|
||||
_control_status{},
|
||||
_filter_initialised(false),
|
||||
_earth_rate_initialised(false),
|
||||
_fuse_height(false),
|
||||
_fuse_pos(false),
|
||||
_fuse_hor_vel(false),
|
||||
_fuse_vert_vel(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),
|
||||
_vel_pos_innov{},
|
||||
_mag_innov{},
|
||||
_heading_innov{},
|
||||
_vel_pos_innov_var{},
|
||||
_mag_innov_var{},
|
||||
_heading_innov_var{}
|
||||
_filter_initialised(false),
|
||||
_earth_rate_initialised(false),
|
||||
_fuse_height(false),
|
||||
_fuse_pos(false),
|
||||
_fuse_hor_vel(false),
|
||||
_fuse_vert_vel(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),
|
||||
_last_disarmed_posD(0.0f),
|
||||
_heading_innov(0.0f),
|
||||
_heading_innov_var(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),
|
||||
_baro_counter(0),
|
||||
_baro_sum(0.0f),
|
||||
_mag_counter(0),
|
||||
_baro_at_alignment(0.0f)
|
||||
{
|
||||
_earth_rate_NED.setZero();
|
||||
_R_prev = matrix::Dcm<float>();
|
||||
_control_status = {};
|
||||
_control_status_prev = {};
|
||||
_state = {};
|
||||
_last_known_posNE.setZero();
|
||||
_earth_rate_NED.setZero();
|
||||
_R_prev = matrix::Dcm<float>();
|
||||
_vel_pos_innov = {};
|
||||
_mag_innov = {};
|
||||
_vel_pos_innov_var = {};
|
||||
_mag_innov_var = {};
|
||||
_delta_angle_corr.setZero();
|
||||
_delta_vel_corr.setZero();
|
||||
_vel_corr.setZero();
|
||||
_last_known_posNE.setZero();
|
||||
_imu_down_sampled = {};
|
||||
_q_down_sampled.setZero();
|
||||
_mag_sum = {};
|
||||
_delVel_sum = {};
|
||||
}
|
||||
|
||||
|
||||
Ekf::~Ekf()
|
||||
{
|
||||
|
||||
|
||||
}
|
||||
|
||||
bool Ekf::init(uint64_t timestamp)
|
||||
{
|
||||
bool ret = initialise_interface(timestamp);
|
||||
@@ -96,7 +115,6 @@ bool Ekf::init(uint64_t timestamp)
|
||||
_output_new.pos.setZero();
|
||||
_output_new.quat_nominal = matrix::Quaternion<float>();
|
||||
|
||||
|
||||
_imu_down_sampled.delta_ang.setZero();
|
||||
_imu_down_sampled.delta_vel.setZero();
|
||||
_imu_down_sampled.delta_ang_dt = 0.0f;
|
||||
@@ -141,7 +159,7 @@ bool Ekf::update()
|
||||
|
||||
// measurement updates
|
||||
|
||||
// Fuse magnetometer data using the selected fuson method and only if angular alignment is complete
|
||||
// Fuse magnetometer data using the selected fusion method and only if angular alignment is complete
|
||||
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
|
||||
if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) {
|
||||
fuseMag();
|
||||
@@ -220,7 +238,7 @@ bool Ekf::initialiseFilter(void)
|
||||
_baro_sum += baro_init.hgt;
|
||||
}
|
||||
|
||||
// check to see if we have enough measruements and return false if not
|
||||
// check to see if we have enough measurements and return false if not
|
||||
if (_baro_counter < 10 || _mag_counter < 10) {
|
||||
return false;
|
||||
|
||||
@@ -283,7 +301,7 @@ void Ekf::predictState()
|
||||
}
|
||||
}
|
||||
|
||||
// attitude error state prediciton
|
||||
// attitude error state prediction
|
||||
matrix::Dcm<float> R_to_earth(_state.quat_nominal); // transformation matrix from body to world frame
|
||||
Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _R_prev * _earth_rate_NED *
|
||||
_imu_sample_delayed.delta_ang_dt;
|
||||
@@ -306,10 +324,8 @@ void Ekf::predictState()
|
||||
constrainStates();
|
||||
}
|
||||
|
||||
|
||||
bool Ekf::collect_imu(imuSample &imu)
|
||||
{
|
||||
|
||||
imu.delta_ang(0) = imu.delta_ang(0) * _state.gyro_scale(0);
|
||||
imu.delta_ang(1) = imu.delta_ang(1) * _state.gyro_scale(1);
|
||||
imu.delta_ang(2) = imu.delta_ang(2) * _state.gyro_scale(2);
|
||||
@@ -317,14 +333,11 @@ bool Ekf::collect_imu(imuSample &imu)
|
||||
imu.delta_ang -= _state.gyro_bias * imu.delta_ang_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f);
|
||||
imu.delta_vel(2) -= _state.accel_z_bias * imu.delta_vel_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f);;
|
||||
|
||||
// store the new sample for the complementary filter prediciton
|
||||
_imu_sample_new = {
|
||||
.delta_ang = imu.delta_ang,
|
||||
.delta_vel = imu.delta_vel,
|
||||
.delta_ang_dt = imu.delta_ang_dt,
|
||||
.delta_vel_dt = imu.delta_vel_dt,
|
||||
.time_us = imu.time_us
|
||||
};
|
||||
_imu_sample_new.delta_ang = imu.delta_ang;
|
||||
_imu_sample_new.delta_vel = imu.delta_vel;
|
||||
_imu_sample_new.delta_ang_dt = imu.delta_ang_dt;
|
||||
_imu_sample_new.delta_vel_dt = imu.delta_vel_dt;
|
||||
_imu_sample_new.time_us = imu.time_us;
|
||||
|
||||
_imu_down_sampled.delta_ang_dt += imu.delta_ang_dt;
|
||||
_imu_down_sampled.delta_vel_dt += imu.delta_vel_dt;
|
||||
@@ -341,13 +354,13 @@ bool Ekf::collect_imu(imuSample &imu)
|
||||
|
||||
if ((_dt_imu_avg * _imu_ticks >= (float)(FILTER_UPDATE_PERRIOD_MS) / 1000) ||
|
||||
_dt_imu_avg * _imu_ticks >= 0.02f) {
|
||||
imu = {
|
||||
.delta_ang = _q_down_sampled.to_axis_angle(),
|
||||
.delta_vel = _imu_down_sampled.delta_vel,
|
||||
.delta_ang_dt = _imu_down_sampled.delta_ang_dt,
|
||||
.delta_vel_dt = _imu_down_sampled.delta_vel_dt,
|
||||
.time_us = imu.time_us
|
||||
};
|
||||
|
||||
imu.delta_ang = _q_down_sampled.to_axis_angle();
|
||||
imu.delta_vel = _imu_down_sampled.delta_vel;
|
||||
imu.delta_ang_dt = _imu_down_sampled.delta_ang_dt;
|
||||
imu.delta_vel_dt = _imu_down_sampled.delta_vel_dt;
|
||||
imu.time_us = imu.time_us;
|
||||
|
||||
_imu_down_sampled.delta_ang.setZero();
|
||||
_imu_down_sampled.delta_vel.setZero();
|
||||
_imu_down_sampled.delta_ang_dt = 0.0f;
|
||||
@@ -422,10 +435,8 @@ void Ekf::calculateOutputStates()
|
||||
_delta_vel_corr = (_state.vel - _output_sample_delayed.vel) * imu_new.delta_vel_dt;
|
||||
|
||||
_vel_corr = (_state.pos - _output_sample_delayed.pos);
|
||||
|
||||
}
|
||||
|
||||
|
||||
void Ekf::fuseAirspeed()
|
||||
{
|
||||
|
||||
|
||||
@@ -86,10 +86,10 @@ public:
|
||||
bool collect_imu(imuSample &imu);
|
||||
|
||||
// this is the current status of the filter control modes
|
||||
filter_control_status_u _control_status = {};
|
||||
filter_control_status_u _control_status;
|
||||
|
||||
// this is the previous status of the filter control modes - used to detect mode transitions
|
||||
filter_control_status_u _control_status_prev = {};
|
||||
filter_control_status_u _control_status_prev;
|
||||
|
||||
// get the ekf WGS-84 origin position and height and the system time it was last set
|
||||
void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt);
|
||||
@@ -97,70 +97,70 @@ public:
|
||||
private:
|
||||
|
||||
static const uint8_t _k_num_states = 24;
|
||||
static constexpr float _k_earth_rate = 0.000072921f;
|
||||
static const float _k_earth_rate = 0.000072921f;
|
||||
|
||||
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;
|
||||
bool _earth_rate_initialised;
|
||||
|
||||
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; // 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
|
||||
|
||||
uint64_t _time_last_fake_gps; // last time in us at which we have faked gps measurement for static mode
|
||||
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 horizotal position measurements was performed (usec)
|
||||
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)
|
||||
Vector2f _last_known_posNE; // last known local NE position vector (m)
|
||||
float _last_disarmed_posD; // vertical position recorded at arming (m)
|
||||
|
||||
Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s
|
||||
Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s
|
||||
|
||||
matrix::Dcm<float> _R_prev; // transformation matrix from earth frame to body frame of previous ekf step
|
||||
matrix::Dcm<float> _R_prev; // transformation matrix from earth frame to body frame of previous ekf step
|
||||
|
||||
float P[_k_num_states][_k_num_states]; // state covariance matrix
|
||||
|
||||
float _vel_pos_innov[6]; // innovations: 0-2 vel, 3-5 pos
|
||||
float _mag_innov[3]; // earth magnetic field innovations
|
||||
float _heading_innov; // heading measurement innovation
|
||||
float _vel_pos_innov[6]; // innovations: 0-2 vel, 3-5 pos
|
||||
float _mag_innov[3]; // earth magnetic field innovations
|
||||
float _heading_innov; // heading measurement innovation
|
||||
|
||||
float _vel_pos_innov_var[6]; // innovation variances: 0-2 vel, 3-5 pos
|
||||
float _mag_innov_var[3]; // earth magnetic field innovation variance
|
||||
float _heading_innov_var; // heading measurement innovation variance
|
||||
float _vel_pos_innov_var[6]; // innovation variances: 0-2 vel, 3-5 pos
|
||||
float _mag_innov_var[3]; // earth magnetic field innovation variance
|
||||
float _heading_innov_var; // heading measurement innovation variance
|
||||
|
||||
float _mag_declination = 0.0f; // magnetic declination used by reset and fusion functions (rad)
|
||||
float _mag_declination; // magnetic declination used by reset and fusion functions (rad)
|
||||
|
||||
// complementary filter states
|
||||
Vector3f _delta_angle_corr; // delta angle correction vector
|
||||
Vector3f _delta_vel_corr; // delta velocity correction vector
|
||||
Vector3f _vel_corr; // velocity correction vector
|
||||
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 _delta_angle_corr; // delta angle correction vector
|
||||
Vector3f _delta_vel_corr; // delta velocity correction vector
|
||||
Vector3f _vel_corr; // velocity correction vector
|
||||
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)
|
||||
|
||||
// variables used for the GPS quality 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
|
||||
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
|
||||
|
||||
// Variables used to publish the WGS-84 location of the EKF local NED origin
|
||||
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)
|
||||
uint64_t _last_gps_origin_time_us; // time the origin was last set (uSec)
|
||||
float _gps_alt_ref; // WGS-84 height (m)
|
||||
|
||||
// Variables used to initialise the filter states
|
||||
uint8_t _baro_counter = 0; // number of baro samples averaged
|
||||
float _baro_sum = 0.0f; // summed baro measurement
|
||||
uint8_t _mag_counter = 0; // number of magnetometer samples averaged
|
||||
Vector3f _mag_sum = {}; // summed magnetometer measurement
|
||||
Vector3f _delVel_sum = {}; // summed delta velocity
|
||||
float _baro_at_alignment; // baro offset relative to alignment position
|
||||
// Variables used to initialize the filter states
|
||||
uint8_t _baro_counter; // number of baro samples averaged
|
||||
float _baro_sum; // summed baro measurement
|
||||
uint8_t _mag_counter; // number of magnetometer samples averaged
|
||||
Vector3f _mag_sum; // summed magnetometer measurement
|
||||
Vector3f _delVel_sum; // summed delta velocity
|
||||
float _baro_at_alignment; // baro offset relative to alignment position
|
||||
|
||||
gps_check_fail_status_u _gps_check_fail_status;
|
||||
|
||||
@@ -168,10 +168,10 @@ private:
|
||||
// and the correction step
|
||||
void calculateOutputStates();
|
||||
|
||||
// initialise filter states of both the delayed ekf and the real time complementary filter
|
||||
// initialize filter states of both the delayed ekf and the real time complementary filter
|
||||
bool initialiseFilter(void);
|
||||
|
||||
// initialise ekf covariance matrix
|
||||
// initialize ekf covariance matrix
|
||||
void initialiseCovariance();
|
||||
|
||||
// predict ekf state
|
||||
@@ -241,15 +241,15 @@ private:
|
||||
// Determine if we are airborne or motors are armed
|
||||
void calculateVehicleStatus();
|
||||
|
||||
// return the square of two foating point numbers - used in autocoded sections
|
||||
// return the square of two floating point numbers - used in auto coded sections
|
||||
inline float sq(float var)
|
||||
{
|
||||
return var * var;
|
||||
}
|
||||
|
||||
// zero the specified range of rows in the state covariance matricx
|
||||
// zero the specified range of rows in the state covariance matrix
|
||||
void zeroRows(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
|
||||
|
||||
// zero the specified range of columns in the state covariance matricx
|
||||
// zero the specified range of columns in the state covariance matrix
|
||||
void zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
|
||||
};
|
||||
|
||||
@@ -215,7 +215,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
||||
_airspeed_buffer.allocate(OBS_BUFFER_LENGTH) &&
|
||||
_flow_buffer.allocate(OBS_BUFFER_LENGTH) &&
|
||||
_output_buffer.allocate(IMU_BUFFER_LENGTH))) {
|
||||
PX4_WARN("Estimator Buffer Allocation failed!");
|
||||
// PX4_WARN("Estimator Buffer Allocation failed!");
|
||||
unallocate_buffers();
|
||||
return false;
|
||||
}
|
||||
|
||||
+30
-34
@@ -32,7 +32,7 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file estimator_base.h
|
||||
* @file estimator_interface.h
|
||||
* Definition of base class for attitude estimators
|
||||
*
|
||||
* @author Roman Bast <bapstroman@gmail.com>
|
||||
@@ -41,7 +41,7 @@
|
||||
|
||||
#include <stdint.h>
|
||||
#include <matrix/matrix/math.hpp>
|
||||
#include <lib/geo/geo.h>
|
||||
#include <geo/geo.h>
|
||||
#include "RingBuffer.h"
|
||||
|
||||
#include "common.h"
|
||||
@@ -81,10 +81,9 @@ public:
|
||||
|
||||
virtual void get_covariances(float *covariances) = 0;
|
||||
|
||||
// get the ekf WGS-84 origin positoin and height and the system time it was last set
|
||||
// get the ekf WGS-84 origin position and height and the system time it was last set
|
||||
virtual void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0;
|
||||
|
||||
|
||||
// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
|
||||
virtual bool collect_gps(uint64_t time_usec, struct gps_message *gps) { return true; }
|
||||
|
||||
@@ -105,6 +104,7 @@ public:
|
||||
|
||||
// set magnetometer data
|
||||
void setMagData(uint64_t time_usec, float *data);
|
||||
//void setMagData(uint64_t time_usec, struct magSample *mag);
|
||||
|
||||
// set gps data
|
||||
void setGpsData(uint64_t time_usec, struct gps_message *gps);
|
||||
@@ -165,15 +165,15 @@ public:
|
||||
|
||||
protected:
|
||||
|
||||
parameters _params; // filter parameters
|
||||
parameters _params; // filter parameters
|
||||
|
||||
static const uint8_t OBS_BUFFER_LENGTH = 10; // defines how many measurement samples we can buffer
|
||||
static const uint8_t IMU_BUFFER_LENGTH = 30; // defines how many imu samples we can buffer
|
||||
static const unsigned FILTER_UPDATE_PERRIOD_MS = 10; // ekf prediction period in milliseconds
|
||||
static const uint8_t OBS_BUFFER_LENGTH = 10; // defines how many measurement samples we can buffer
|
||||
static const uint8_t IMU_BUFFER_LENGTH = 30; // defines how many imu samples we can buffer
|
||||
static const unsigned FILTER_UPDATE_PERRIOD_MS = 10; // ekf prediction period in milliseconds
|
||||
|
||||
float _dt_imu_avg; // average imu update period in s
|
||||
float _dt_imu_avg; // 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;
|
||||
@@ -183,26 +183,25 @@ protected:
|
||||
airspeedSample _airspeed_sample_delayed;
|
||||
flowSample _flow_sample_delayed;
|
||||
|
||||
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
|
||||
|
||||
uint64_t _imu_ticks; // counter for imu updates
|
||||
uint64_t _imu_ticks; // counter for imu updates
|
||||
|
||||
bool _imu_updated = false; // true if the ekf should update (completed downsampling process)
|
||||
bool _initialised = false; // true if ekf interface instance (data buffering) is initialised
|
||||
bool _vehicle_armed = false; // vehicle arm status used to turn off functionality used on the ground
|
||||
bool _in_air = false; // we assume vehicle is in the air, set by the given landing detector
|
||||
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 _vehicle_armed; // vehicle arm status used to turn off functionality used on the ground
|
||||
bool _in_air; // we assume vehicle is in the air, set by the given landing detector
|
||||
|
||||
bool _NED_origin_initialised = false;
|
||||
bool _gps_speed_valid = false;
|
||||
float _gps_speed_accuracy = 0.0f; // GPS receiver reported speed accuracy (m/s)
|
||||
struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians)
|
||||
bool _NED_origin_initialised;
|
||||
bool _gps_speed_valid;
|
||||
float _gps_speed_accuracy; // GPS receiver reported speed accuracy (m/s)
|
||||
struct map_projection_reference_s _pos_ref; // Contains WGS-84 position latitude and longitude (radians)
|
||||
|
||||
bool _mag_healthy = false; // computed by mag innovation test
|
||||
bool _mag_healthy; // computed by mag innovation test
|
||||
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
|
||||
|
||||
// data buffer instances
|
||||
@@ -215,14 +214,13 @@ protected:
|
||||
RingBuffer<flowSample> _flow_buffer;
|
||||
RingBuffer<outputSample> _output_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_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
|
||||
|
||||
|
||||
fault_status_t _fault_status;
|
||||
|
||||
// allocate data buffers and intialise interface variables
|
||||
@@ -231,8 +229,6 @@ protected:
|
||||
// free buffer memory
|
||||
void unallocate_buffers();
|
||||
|
||||
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)
|
||||
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)
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user