From 6c96f45f085feaa5e729cf1cdb921a99f0bd25cd Mon Sep 17 00:00:00 2001 From: mcsauder Date: Tue, 23 Feb 2016 19:49:27 -0700 Subject: [PATCH] Remove whitespace differences with upstream for pull request. --- EKF/common.h | 80 +++++++++++++++++++-------------------- EKF/ekf.cpp | 1 + EKF/ekf.h | 42 ++++++++++---------- EKF/estimator_interface.h | 31 +++++++-------- 4 files changed, 78 insertions(+), 76 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index dd3b96f68f..c8fed865b3 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -67,45 +67,45 @@ typedef matrix::Quaternion Quaternion; typedef matrix::Matrix 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 diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 503af3c837..d6f8241982 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -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(); diff --git a/EKF/ekf.h b/EKF/ekf.h index cee50b7f06..c68e818a44 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -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 _R_prev; // transformation matrix from earth frame to body frame of previous ekf step + matrix::Dcm _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 diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 2bc98222a6..07920aa7ea 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -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 _flow_buffer; RingBuffer _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;