fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-13 08:57:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Remove whitespace differences with upstream for pull request.

Browse Source
This commit is contained in:
mcsauder
2016-02-23 19:49:27 -07:00
parent fad1c87631
commit 6c96f45f08
4 changed files with 78 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
+40 -40
View File
@@ -67,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 {
@@ -217,28 +217,28 @@ struct parameters {
#define MAG_FUSE_TYPE_2D 3 // A 2D fusion that uses the horizontal projection of the magnetic fields measurement will alays be used. This is less accurate, but less affected by 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
EKF/ekf.cpp
+1
View File
@@ -83,6 +83,7 @@ Ekf::Ekf():
_mag_innov_var = {};
_delta_angle_corr.setZero();
_delta_vel_corr.setZero();
_last_known_posNE.setZero();
_vel_corr.setZero();
_imu_down_sampled = {};
_q_down_sampled.setZero();
EKF/ekf.h
+21 -21
View File
@@ -100,17 +100,17 @@ private:
static const uint8_t _k_num_states = 24;
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 horizontal position measurements was performed (usec)
uint64_t _time_last_vel_fuse; // time the last fusion of velocity measurements was performed (usec)
@@ -119,28 +119,28 @@ private:
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; // 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; // GPS north position derivative (m/s)
@@ -169,10 +169,10 @@ private:
// and the correction step
void calculateOutputStates();
// initialize filter states of both the delayed ekf and the real time complementary filter
// initialise filter states of both the delayed ekf and the real time complementary filter
bool initialiseFilter(void);
// initialize ekf covariance matrix
// initialise ekf covariance matrix
void initialiseCovariance();
// predict ekf state
EKF/estimator_interface.h
+16 -15
View File
@@ -164,15 +164,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;
@@ -182,11 +182,11 @@ 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; // true if the ekf should update (completed downsampling process)
bool _initialised; // true if the ekf interface instance (data buffering) is initialized
@@ -201,6 +201,7 @@ protected:
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
@@ -213,11 +214,11 @@ 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;