diff --git a/EKF/RingBuffer.h b/EKF/RingBuffer.h index 1704f5d56b..446151764b 100644 --- a/EKF/RingBuffer.h +++ b/EKF/RingBuffer.h @@ -82,13 +82,8 @@ public: } } - inline void push(data_type sample, bool debug = false) + inline void push(data_type sample) { - if (debug) { - printf("elapsed %" PRIu64 "\n", sample.time_us - _time_last); - _time_last = sample.time_us; - } - int head_new = _head; if (_first_write) { diff --git a/EKF/common.h b/EKF/common.h index 22210512fa..aa8da89ae8 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -64,45 +64,45 @@ typedef matrix::Quaternion Quaternion; typedef matrix::Matrix Matrix3f; struct outputSample { - Quaternion quat_nominal; - Vector3f vel; - Vector3f pos; - uint64_t time_us; + 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; - Vector3f delta_vel; - float delta_ang_dt; - float delta_vel_dt; - uint64_t time_us; + 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; - float hgt; - Vector3f vel; - uint64_t time_us; + 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; - uint64_t time_us; + Vector3f mag; // NED magnetometer body frame measurements + uint64_t time_us; // timestamp in microseconds }; struct baroSample { - float hgt; - uint64_t time_us; + float hgt; // barometer height above sea level measurement in m + uint64_t time_us; // timestamp in microseconds }; struct rangeSample { - float rng; - uint64_t time_us; + float rng; // range (distance to ground) measurement in m + uint64_t time_us; // timestamp in microseconds }; struct airspeedSample { - float airspeed; - uint64_t time_us; + float airspeed; // airspeed measurement in m/s + uint64_t time_us; // timestamp in microseconds }; struct flowSample { @@ -155,16 +155,16 @@ struct parameters { }; struct stateSample { - Vector3f ang_error; - Vector3f vel; - Vector3f pos; - Vector3f gyro_bias; - Vector3f gyro_scale; - float accel_z_bias; - Vector3f mag_I; - Vector3f mag_B; - Vector2f wind_vel; - Quaternion quat_nominal; + 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 { diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 32e4b87c47..599b4c6650 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -40,7 +40,6 @@ */ #include "ekf.h" -#include Ekf::Ekf(): _control_status{}, @@ -443,83 +442,3 @@ void Ekf::fuseRange() { } - -void Ekf::printStates() -{ - static int counter = 0; - - if (counter % 50 == 0) { - printf("quaternion\n"); - - for (int i = 0; i < 4; i++) { - printf("quat %i %.5f\n", i, (double)_state.quat_nominal(i)); - } - - matrix::Euler euler(_state.quat_nominal); - printf("yaw pitch roll %.5f %.5f %.5f\n", (double)euler(2), (double)euler(1), (double)euler(0)); - - printf("vel\n"); - - for (int i = 0; i < 3; i++) { - printf("v %i %.5f\n", i, (double)_state.vel(i)); - } - - printf("pos\n"); - - for (int i = 0; i < 3; i++) { - printf("p %i %.5f\n", i, (double)_state.pos(i)); - } - - printf("gyro_scale\n"); - - for (int i = 0; i < 3; i++) { - printf("gs %i %.5f\n", i, (double)_state.gyro_scale(i)); - } - - printf("mag earth\n"); - - for (int i = 0; i < 3; i++) { - printf("mI %i %.5f\n", i, (double)_state.mag_I(i)); - } - - printf("mag bias\n"); - - for (int i = 0; i < 3; i++) { - printf("mB %i %.5f\n", i, (double)_state.mag_B(i)); - } - - counter = 0; - } - - counter++; - -} - -void Ekf::printStatesFast() -{ - static int counter_fast = 0; - - if (counter_fast % 50 == 0) { - printf("quaternion\n"); - - for (int i = 0; i < 4; i++) { - printf("quat %i %.5f\n", i, (double)_output_new.quat_nominal(i)); - } - - printf("vel\n"); - - for (int i = 0; i < 3; i++) { - printf("v %i %.5f\n", i, (double)_output_new.vel(i)); - } - - printf("pos\n"); - - for (int i = 0; i < 3; i++) { - printf("p %i %.5f\n", i, (double)_output_new.pos(i)); - } - - counter_fast = 0; - } - - counter_fast++; -} diff --git a/EKF/ekf.h b/EKF/ekf.h index a630f3bc3b..8fb2f09e5d 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -49,7 +49,10 @@ public: Ekf(); ~Ekf(); + // initialise variables to sane values (also interface class) bool init(uint64_t timestamp); + + // should be called every time new data is pushed into the filter bool update(); // gets the innovations of velocity and position measurements @@ -92,7 +95,7 @@ private: static const uint8_t _k_num_states = 24; static constexpr float _k_earth_rate = 0.000072921f; - stateSample _state; + stateSample _state; // state struct of the ekf running at the delayed time horizon bool _filter_initialised; bool _earth_rate_initialised; @@ -102,7 +105,7 @@ private: 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; + 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_vel_fuse; // time the last fusion of velocity measurements was performed (usec) @@ -111,9 +114,9 @@ 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; + Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s - matrix::Dcm _R_prev; + 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 @@ -126,11 +129,11 @@ private: float _heading_innov_var; // heading measurement innovation variance // complementary filter states - Vector3f _delta_angle_corr; - Vector3f _delta_vel_corr; - Vector3f _vel_corr; - imuSample _imu_down_sampled; - Quaternion _q_down_sampled; + 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) @@ -155,52 +158,65 @@ private: gps_check_fail_status_u _gps_check_fail_status; + // update the real time complementary filter states. This includes the prediction + // and the correction step void calculateOutputStates(); + // initialise filter states of both the delayed ekf and the real time complementary filter bool initialiseFilter(void); + // initialise ekf covariance matrix void initialiseCovariance(); + // predict ekf state void predictState(); + // predict ekf covariance void predictCovariance(); + // ekf sequential fusion of magnetometer measurements void fuseMag(); + // fuse magnetometer heading measurement void fuseHeading(); + // fuse magnetometer declination measurement void fuseDeclination(); + // fuse airspeed measurement void fuseAirspeed(); + // fuse range measurements void fuseRange(); + // fuse velocity and position measurements (also barometer height) void fuseVelPosHeight(); + // reset velocity states of the ekf void resetVelocity(); + // reset position states of the ekf (only vertical position) void resetPosition(); + // reset height state of the ekf void resetHeight(); void makeCovSymetrical(); + // limit the diagonal of the covariance matrix void limitCov(); - void printCovToFile(char const *filename); - - void assertCovNiceness(); - + // make ekf covariance matrix symmetric void makeSymmetrical(); + // constrain the ekf states void constrainStates(); + // generic function which will perform a fusion step given a kalman gain K + // and a scalar innovation value void fuse(float *K, float innovation); - void printStates(); - - void printStatesFast(); - + // calculate the earth rotation vector from a given latitude void calcEarthRateNED(Vector3f &omega, double lat_rad) const; // return true id the GPS quality is good enough to set an origin and start aiding diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 5796cd8628..5cf2ced430 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -94,39 +94,6 @@ void Ekf::resetHeight() } } -#if defined(__PX4_POSIX) && !defined(__PX4_QURT) -void Ekf::printCovToFile(char const *filename) -{ - std::ofstream myfile; - myfile.open(filename); - myfile << "Covariance matrix\n"; - myfile << std::setprecision(1); - - for (int i = 0; i < _k_num_states; i++) { - for (int j = 0; j < _k_num_states; j++) { - myfile << std::to_string(P[i][j]) << std::setprecision(1) << " "; - } - - myfile << "\n\n\n\n\n\n\n\n\n\n"; - } -} -#endif - -// This checks if the diagonal of the covariance matrix is non-negative -// and that the matrix is symmetric -void Ekf::assertCovNiceness() -{ - for (int row = 0; row < _k_num_states; row++) { - for (int column = 0; column < row; column++) { - assert(fabsf(P[row][column] - P[column][row]) < 0.00001f); - } - } - - for (int i = 0; i < _k_num_states; i++) { - assert(P[i][i] > -0.000001f); - } -} - // This function forces the covariance matrix to be symmetric void Ekf::makeSymmetrical() { diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 858e136dda..cf26c748d7 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -223,7 +223,6 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) _dt_imu_avg = 0.0f; - _imu_time_last = timestamp; _imu_sample_delayed.delta_ang.setZero(); _imu_sample_delayed.delta_vel.setZero(); @@ -265,78 +264,3 @@ bool EstimatorInterface::position_is_valid() // TOTO implement proper check based on published GPS accuracy, innovaton consistency checks and timeout status return _NED_origin_initialised && (_time_last_imu - _time_last_gps) < 5e6; } - -void EstimatorInterface::printStoredIMU() -{ - printf("---------Printing IMU data buffer------------\n"); - - for (int i = 0; i < IMU_BUFFER_LENGTH; i++) { - printIMU(&_imu_buffer[i]); - } -} - -void EstimatorInterface::printIMU(struct imuSample *data) -{ - printf("time %" PRIu64 "\n", data->time_us); - printf("delta_ang_dt %.5f\n", (double)data->delta_ang_dt); - printf("delta_vel_dt %.5f\n", (double)data->delta_vel_dt); - printf("dA: %.5f %.5f %.5f \n", (double)data->delta_ang(0), (double)data->delta_ang(1), (double)data->delta_ang(2)); - printf("dV: %.5f %.5f %.5f \n\n", (double)data->delta_vel(0), (double)data->delta_vel(1), (double)data->delta_vel(2)); -} - -void EstimatorInterface::printQuaternion(Quaternion &q) -{ - printf("q1 %.5f q2 %.5f q3 %.5f q4 %.5f\n", (double)q(0), (double)q(1), (double)q(2), (double)q(3)); -} - -void EstimatorInterface::print_imu_avg_time() -{ - printf("dt_avg: %.5f\n", (double)_dt_imu_avg); -} - -void EstimatorInterface::printStoredMag() -{ - printf("---------Printing mag data buffer------------\n"); - - for (int i = 0; i < OBS_BUFFER_LENGTH; i++) { - printMag(&_mag_buffer[i]); - } -} - -void EstimatorInterface::printMag(struct magSample *data) -{ - printf("time %" PRIu64 "\n", data->time_us); - printf("mag: %.5f %.5f %.5f \n\n", (double)data->mag(0), (double)data->mag(1), (double)data->mag(2)); - -} - -void EstimatorInterface::printBaro(struct baroSample *data) -{ - printf("time %" PRIu64 "\n", data->time_us); - printf("baro: %.5f\n\n", (double)data->hgt); -} - -void EstimatorInterface::printStoredBaro() -{ - printf("---------Printing baro data buffer------------\n"); - - for (int i = 0; i < OBS_BUFFER_LENGTH; i++) { - printBaro(&_baro_buffer[i]); - } -} - -void EstimatorInterface::printGps(struct gpsSample *data) -{ - printf("time %" PRIu64 "\n", data->time_us); - printf("gps pos: %.5f %.5f %.5f\n", (double)data->pos(0), (double)data->pos(1), (double)data->hgt); - printf("gps vel %.5f %.5f %.5f\n\n", (double)data->vel(0), (double)data->vel(1), (double)data->vel(2)); -} - -void EstimatorInterface::printStoredGps() -{ - printf("---------Printing GPS data buffer------------\n"); - - for (int i = 0; i < OBS_BUFFER_LENGTH; i++) { - printGps(&_gps_buffer[i]); - } -} diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 175d3ab185..bfc7767eea 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -128,17 +128,6 @@ public: // set vehicle arm status data void set_arm_status(bool data) { _vehicle_armed = data; } - void printIMU(struct imuSample *data); - void printStoredIMU(); - void printQuaternion(Quaternion &q); - void print_imu_avg_time(); - void printMag(struct magSample *data); - void printStoredMag(); - void printBaro(struct baroSample *data); - void printStoredBaro(); - void printGps(struct gpsSample *data); - void printStoredGps(); - bool position_is_valid(); @@ -162,22 +151,22 @@ public: } void copy_timestamp(uint64_t *time_us) { - *time_us = _imu_time_last; + *time_us = _time_last_imu; } protected: parameters _params; // filter parameters - static const uint8_t OBS_BUFFER_LENGTH = 10; - static const uint8_t IMU_BUFFER_LENGTH = 30; - static const unsigned FILTER_UPDATE_PERRIOD_MS = 10; + 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; - uint64_t _imu_time_last; + float _dt_imu_avg; // average imu update period in s - imuSample _imu_sample_delayed; + 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; @@ -185,14 +174,14 @@ protected: airspeedSample _airspeed_sample_delayed; flowSample _flow_sample_delayed; - outputSample _output_sample_delayed; - outputSample _output_new; - imuSample _imu_sample_new; + 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; + uint64_t _imu_ticks; // counter for imu updates - bool _imu_updated = false; - bool _initialised = false; + 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 _NED_origin_initialised = false; @@ -206,6 +195,7 @@ protected: float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios + // data buffer instances RingBuffer _imu_buffer; RingBuffer _gps_buffer; RingBuffer _mag_buffer; @@ -215,15 +205,19 @@ protected: RingBuffer _flow_buffer; RingBuffer _output_buffer; - uint64_t _time_last_imu; - uint64_t _time_last_gps; - uint64_t _time_last_mag; - uint64_t _time_last_baro; - uint64_t _time_last_range; - uint64_t _time_last_airspeed; + 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 bool initialise_interface(uint64_t timestamp); + + // free buffer memory void unallocate_buffers(); };