From 8de8b0eb76d38297b8a461d5ee3bbd21a76c546a Mon Sep 17 00:00:00 2001 From: Roman Date: Mon, 7 Dec 2015 09:26:30 +0100 Subject: [PATCH] prediction and vel pos heading fusion working --- CMakeLists.txt | 2 + EKF/covariance.cpp | 202 ++++++++++++++++++++++++++++++----------- EKF/ekf.cpp | 109 +++++++++++++++++----- EKF/ekf.h | 39 +++++++- EKF/ekf_helper.cpp | 96 ++++++++++++++++++++ EKF/estimator_base.cpp | 47 +++++----- EKF/estimator_base.h | 90 ++++++++++++------ EKF/heading_fusion.cpp | 182 +++++++++++++++++++++++++++++++++++++ EKF/vel_pos_fusion.cpp | 162 +++++++++++++++++++++++++++++++++ 9 files changed, 794 insertions(+), 135 deletions(-) create mode 100644 EKF/heading_fusion.cpp create mode 100644 EKF/vel_pos_fusion.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 57219ef845..009295fb79 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,5 +46,7 @@ px4_add_module( EKF/ekf.cpp EKF/ekf_helper.cpp EKF/covariance.cpp + EKF/vel_pos_fusion.cpp + EKF/heading_fusion.cpp ) # vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 0ad53f2a34..4de91b39fa 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -42,37 +42,38 @@ #include "ekf.h" #include + #include #define sq(_arg) powf(_arg, 2.0f) void Ekf::initialiseCovariance() { - for (unsigned i = 0; i < _kNumStates; i++) { - for (unsigned j = 0; j < _kNumStates; j++) { + for (unsigned i = 0; i < _k_num_states; i++) { + for (unsigned j = 0; j < _k_num_states; j++) { P[i][j] = 0.0f; } } // XXX use initial guess for the diagonal elements for the covariance matrix // angle error - P[0][0] = 0.1f; - P[1][1] = 0.1f; - P[2][2] = 0.1f; + P[0][0] = 0.001f; + P[1][1] = 0.001f; + P[2][2] = 0.001f; // velocity - P[3][3] = 9.0f; - P[4][4] = 9.0f; - P[5][5] = 9.0f; + P[3][3] = 0.1f; + P[4][4] = 0.1f; + P[5][5] = 0.1f; // position - P[6][6] = 9.0f; - P[7][7] = 9.0f; - P[8][8] = 9.0f; + P[6][6] = 0.1f; + P[7][7] = 0.1f; + P[8][8] = 0.1f; // gyro bias - P[9][9] = 0.001f; - P[10][10] = 0.001f; - P[11][11] = 0.001f; + P[9][9] = 0.00001f; + P[10][10] = 0.00001f; + P[11][11] = 0.00001f; // gyro scale P[12][12] = 0.0001f; @@ -93,8 +94,8 @@ void Ekf::initialiseCovariance() P[21][21] = 0.0001f; // wind - P[22][22] = 4.0f; - P[23][23] = 4.0f; + P[22][22] = 0.01f; + P[23][23] = 0.01f; } @@ -124,28 +125,50 @@ void Ekf::predictCovariance() float dvz_b = _state.accel_z_bias; - float dt = _imu_sample_delayed.delta_ang_dt; + float dt = _imu_sample_delayed.delta_vel_dt; + + // compute process noise + float process_noise[_k_num_states] = {}; + + float d_ang_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1e-4f); + float d_vel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 1e-6f, 1e-2f); + float d_ang_scale_sig = dt * math::constrain(_params.gyro_scale_p_noise, 1e-6f, 1e-2f); + float mag_I_sig = dt * math::constrain(_params.mag_p_noise, 1e-4f, 1e-1f); + float mag_B_sig = dt * math::constrain(_params.mag_p_noise, 1e-4f, 1e-1f); + float wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.01f, 1.0f); + + for (unsigned i = 0; i < 9; i++) { + process_noise[i] = 0.0f; + } + for (unsigned i = 9; i < 12; i++) { + process_noise[i] = sq(d_ang_bias_sig); + } + for (unsigned i = 12; i < 15; i++) { + process_noise[i] = sq(d_ang_scale_sig); + } + process_noise[15] = sq(d_vel_bias_sig); + + for (unsigned i = 16; i < 19; i++) { + process_noise[i] = sq(mag_I_sig); + } + for (unsigned i = 19; i < 22; i++) { + process_noise[i] = sq(mag_B_sig); + } + for (unsigned i = 22; i < 24; i++) { + process_noise[i] = sq(wind_vel_sig); + } + // assign input noise // inputs to the system are 3 delta angles and 3 delta velocities - float daxNoise = _params.dax_noise; - float dayNoise = _params.day_noise; - float dazNoise = _params.daz_noise; + float daxNoise, dayNoise, dazNoise; + float dvxNoise, dvyNoise, dvzNoise; + float gyro_noise = math::constrain(_params.gyro_noise, 1e-4f, 1e-2f); + daxNoise = dayNoise = dazNoise = dt * gyro_noise; + float accel_noise = math::constrain(_params.accel_noise, 1e-2f, 1.0f); + dvxNoise = dvyNoise = dvzNoise = dt * accel_noise; - float dvxNoise = _params.dvx_noise; - float dvyNoise = _params.dvy_noise; - float dvzNoise = _params.dvz_noise; - - // assign process noise variables - float delta_ang_sig = _params.delta_ang_sig; - float delta_vel_sig = _params.delta_vel_sig; - float delta_pos_sig = _params.delta_pos_sig; - float delta_gyro_bias_sig = _params.delta_gyro_bias_sig; - float delta_gyro_scale_sig = _params.delta_gyro_scale_sig; - float delta_vel_bias_z_sig = _params.delta_vel_bias_z_sig; - float delta_mag_body_sig = _params.delta_mag_body_sig; - float delta_mag_earth_sig = _params.delta_mag_earth_sig; - float delta_wind_sig = _params.delta_wind_sig; + // predict covarinace matrix // intermediate calculations float SF[25] = {}; @@ -632,35 +655,104 @@ void Ekf::predictCovariance() nextP[22][23] = P[22][23]; nextP[23][23] = P[23][23]; - // add process noise to diagonal - for (int i = 0; i < 3; i++) {nextP[i][i] += delta_ang_sig * delta_ang_sig;} + // add process noise + for (unsigned i = 0; i < _k_num_states; i++) { + nextP[i][i] += process_noise[i]; + } - for (int i = 3; i < 6; i++) {nextP[i][i] += delta_vel_sig * delta_vel_sig;} - - for (int i = 6; i < 9; i++) {nextP[i][i] += delta_vel_sig * delta_pos_sig;} - - for (int i = 9; i < 12; i++) {nextP[i][i] += delta_gyro_bias_sig * delta_gyro_bias_sig;} - - for (int i = 12; i < 15; i++) {nextP[i][i] += delta_gyro_scale_sig * delta_gyro_scale_sig;} - - nextP[15][15] += delta_vel_bias_z_sig * delta_vel_bias_z_sig; - - for (int i = 16; i < 19; i++) {nextP[i][i] += delta_mag_earth_sig * delta_mag_earth_sig;} - - for (int i = 19; i < 22; i++) {nextP[i][i] += delta_mag_body_sig * delta_mag_body_sig;} - - for (int i = 19; i < 24; i++) {nextP[i][i] += delta_wind_sig * delta_wind_sig;} + // stop position covariance growth if our total position variance reaches 100m + // this can happen if we loose gps for some time + if ((P[6][6] + P[7][7]) > 1e4f) { + for (uint8_t i = 6; i < 8; i++) { + for (uint8_t j = 0; j < _k_num_states; j++) { + nextP[i][j] = P[i][j]; + nextP[j][i] = P[j][i]; + } + } + } // covariance matrix is symmetrical, so copy upper half to lower half - for (unsigned row = 1; row < _kNumStates; row++) { + for (unsigned row = 1; row < _k_num_states; row++) { for (unsigned column = 0 ; column < row; column++) { nextP[row][column] = nextP[column][row]; } } - for (unsigned row = 0; row < _kNumStates; row++) { - for (unsigned column = 0; column < _kNumStates; column++) { - P[row][column] = nextP[row][column]; + for (unsigned i = 0; i < _k_num_states; i++) { + P[i][i] = nextP[i][i]; + } + + for (unsigned row = 1; row < _k_num_states; row++) { + for (unsigned column = 0; column < row; column++) { + P[row][column] = 0.5f * (nextP[row][column] + nextP[column][row]); + P[column][row] = P[row][column]; } } + + limitCov(); } + +void Ekf::limitCov() +{ + // Covariance diagonal limits. Use same values for states which + // belong to the same group (e.g. vel_x, vel_y, vel_z) + float P_lim[9] = {}; + P_lim[0] = 1.0f; // angle error max var + P_lim[1] = 1000.0f; // velocity max var + P_lim[2] = 1000000.0f; // positiion max var + P_lim[3] = 0.001f; // gyro bias max var + P_lim[4] = 0.01f; // gyro scale max var + P_lim[5] = 0.1f; // delta velocity z bias max var + P_lim[6] = 0.01f; // earth mag field max var + P_lim[7] = 0.01f; // body mag field max var + P_lim[8] = 1000.0f; // wind max var + + for (int i = 0; i < 3; i++) { + + math::constrain(P[i][i], 0.0f, P_lim[0]); + } + + for (int i = 3; i < 6; i++) { + + math::constrain(P[i][i], 0.0f, P_lim[1]); + } + + for (int i = 6; i < 9; i++) { + + + math::constrain(P[i][i], 0.0f, P_lim[2]); + } + + for (int i = 9; i < 12; i++) { + + + math::constrain(P[i][i], 0.0f, P_lim[3]); + } + + for (int i = 12; i < 15; i++) { + + + math::constrain(P[i][i], 0.0f, P_lim[4]); + } + + + math::constrain(P[15][15], 0.0f, P_lim[5]); + + for (int i = 16; i < 19; i++) { + + math::constrain(P[i][i], 0.0f, P_lim[6]); + } + + for (int i = 19; i < 22; i++) { + + + math::constrain(P[i][i], 0.0f, P_lim[7]); + } + + for (int i = 22; i < 24; i++) { + + math::constrain(P[i][i], 0.0f, P_lim[8]); + } +} + + diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 260bba6153..d21dd99e26 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -42,9 +42,15 @@ #include "ekf.h" #include -Ekf::Ekf() +Ekf::Ekf(): + _filter_initialised(false), + _earth_rate_initialised(false), + _fuse_height(false), + _fuse_pos(false), + _fuse_vel(false) { - + _earth_rate_NED.setZero(); + _R_prev = matrix::Dcm(); } @@ -53,22 +59,24 @@ Ekf::~Ekf() } -void Ekf::update() +bool Ekf::update() { + bool ret = false; // indicates if there has been an update if (!_filter_initialised) { _filter_initialised = initialiseFilter(); } - + printStates(); // prediction if (_imu_updated) { + ret = true; predictState(); predictCovariance(); - _imu_updated = false; } // measurement updates + if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { - fuseMag(); + fuseHeading(); } if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { @@ -80,8 +88,10 @@ void Ekf::update() _fuse_vel = true; } + if (_fuse_height || _fuse_pos || _fuse_vel) { - fusePosVel(); + fuseVelPosHeight(); + _fuse_vel = _fuse_pos = _fuse_height = false; } if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) { @@ -92,11 +102,20 @@ void Ekf::update() fuseAirspeed(); } + // write to output if this has been a prediction step + if (_imu_updated) { + _output_delayed.vel = _state.vel; + _output_delayed.pos = _state.pos; + _output_delayed.quat_nominal = _state.quat_nominal; + _output_delayed.time_us = _imu_time_last; + _imu_updated = false; + } + + return ret; } bool Ekf::initialiseFilter(void) { - _state.ang_error.setZero(); _state.vel.setZero(); _state.pos.setZero(); @@ -133,39 +152,36 @@ bool Ekf::initialiseFilter(void) void Ekf::predictState() { - // compute transformation matrix from body to world frame - matrix::Dcm R(_state.quat_nominal); - R.transpose(); + if (!_earth_rate_initialised) { + if (_gps_initialised) { + calcEarthRateNED(_earth_rate_NED, _posRef.lat_rad ); + _earth_rate_initialised = true; + } + } // attitude error state prediciton - Quaternion dq; - dq.from_axis_angle(_imu_sample_delayed.delta_ang); + matrix::Dcm 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; + Quaternion dq; // delta quaternion since last update + dq.from_axis_angle(corrected_delta_ang); _state.quat_nominal = dq * _state.quat_nominal; _state.quat_nominal.normalize(); + _R_prev = R_to_earth.transpose(); + Vector3f vel_last = _state.vel; // predict velocity states - _state.vel += R * _imu_sample_delayed.delta_vel; + _state.vel += R_to_earth * _imu_sample_delayed.delta_vel; _state.vel(2) += 9.81f * _imu_sample_delayed.delta_vel_dt; // predict position states via trapezoidal integration of velocity _state.pos += (vel_last + _state.vel) * _imu_sample_delayed.delta_vel_dt * 0.5f; - //matrix::Euler euler(_state.quat_nominal); - //printf("roll pitch yaw %.5f %.5f %.5f\n", (double)euler(2), (double)euler(1), (double)euler(0)); + constrainStates(); } -void Ekf::fusePosVel() -{ - -} - -void Ekf::fuseMag() -{ -} - void Ekf::fuseAirspeed() { @@ -175,3 +191,46 @@ void Ekf::fuseRange() { } + +void Ekf::printStates() +{ + static int counter = 0; + + if (counter % 50 == 0) { + printf("ang error\n"); + for(int i = 0; i < 3; i++) { + printf("ang_e %i %.5f\n", i, (double)_state.ang_error(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("g gyro_bias\n"); + for(int i = 0; i < 3; i++) { + printf("gb %i %.5f\n", i, (double)_state.gyro_bias(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_I\n"); + for(int i = 0; i < 3; i++) { + printf("mI %i %.5f\n", i, (double)_state.mag_I(i)); + } + counter = 0; + } + counter++; + +} diff --git a/EKF/ekf.h b/EKF/ekf.h index f13e5a8be6..739a65686c 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -41,6 +41,8 @@ #include "estimator_base.h" +#define sq(_arg) powf(_arg, 2.0f) + class Ekf : public EstimatorBase { public: @@ -48,18 +50,25 @@ public: Ekf(); ~Ekf(); - void update(); + bool update(); private: - static const uint8_t _kNumStates = 24; + static const uint8_t _k_num_states = 24; + static constexpr float _k_earth_rate = 0.000072921f; + bool _filter_initialised; + bool _earth_rate_initialised; bool _fuse_height; // true if there is new baro data bool _fuse_pos; // true if there is new position data from gps bool _fuse_vel; // true if there is new velocity data from gps - float P[_kNumStates][_kNumStates]; // state covariance matrix + Vector3f _earth_rate_NED; + + matrix::Dcm _R_prev; + + float P[_k_num_states][_k_num_states]; // state covariance matrix bool initialiseFilter(void); @@ -69,17 +78,37 @@ private: void predictCovariance(); - void fusePosVel(); + void fuseMag(uint8_t index); - void fuseMag(); + void fuseHeading(); void fuseAirspeed(); void fuseRange(); + void fuseVelPosHeight(); + void resetVelocity(); void resetPosition(); + void makeCovSymetrical(); + + void limitCov(); + + void printCovToFile(char const *filename); + + void assertCovNiceness(); + + void makeSymmetrical(); + + void constrainStates(); + + void fuse(float *K, float innovation); + + void printStates(); + + void calcEarthRateNED(Vector3f &omega, double lat_rad) const; + }; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 7ecec33af2..ef9727cedc 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -40,7 +40,13 @@ */ #include "ekf.h" +#include +#include +#include +#include +// Reset the velocity states. If we have a recent and valid +// gps measurement then use for velocity initialisation void Ekf::resetVelocity() { // if we have a valid GPS measurement use it to initialise velocity states @@ -54,6 +60,8 @@ void Ekf::resetVelocity() } } +// Reset position states. If we have a recent and valid +// gps measurement then use for position initialisation void Ekf::resetPosition() { // if we have a valid GPS measurement use it to initialise position states @@ -70,3 +78,91 @@ void Ekf::resetPosition() baroSample baro_newest = _baro_buffer.get_newest(); _state.pos(2) = baro_newest.hgt; } + +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"; + } +} + +// 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() +{ + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < row; column++) { + float tmp = (P[row][column] + P[column][row]) / 2; + P[row][column] = tmp; + P[column][row] = tmp; + } + } +} + +void Ekf::constrainStates() +{ + for (int i = 0; i < 3; i++) { + _state.ang_error(i) = math::constrain(_state.ang_error(i), -1.0f, 1.0f); + } + + for (int i = 0; i < 3; i++) { + _state.vel(i) = math::constrain(_state.vel(i), -1000.0f, 1000.0f); + } + + for (int i = 0; i < 3; i++) { + _state.pos(i) = math::constrain(_state.pos(i), -1.e6f, 1.e6f); + } + + for (int i = 0; i < 3; i++) { + _state.gyro_bias(i) = math::constrain(_state.gyro_bias(i), -0.349066f * _dt_imu_avg, 0.349066f * _dt_imu_avg); + } + + for (int i = 0; i < 3; i++) { + _state.gyro_scale(i) = math::constrain(_state.gyro_scale(i), 0.95f, 1.05f); + } + + _state.accel_z_bias = math::constrain(_state.accel_z_bias, -1.0f * _dt_imu_avg, 1.0f * _dt_imu_avg); + + for (int i = 0; i < 3; i++) { + _state.mag_I(i) = math::constrain(_state.mag_I(i), -1.0f, 1.0f); + } + + for (int i = 0; i < 3; i++) { + _state.mag_B(i) = math::constrain(_state.mag_B(i), -0.5f, 0.5f); + } + + for (int i = 0; i < 3; i++) { + _state.wind_vel(i) = math::constrain(_state.wind_vel(i), -100.0f, 100.0f); + } +} + +// calculate the earth rotation vector +void Ekf::calcEarthRateNED(Vector3f &omega, double lat_rad) const +{ + omega(0) = _k_earth_rate * cosf((float)lat_rad); + omega(1) = 0.0f; + omega(2) = -_k_earth_rate * sinf((float)lat_rad); +} diff --git a/EKF/estimator_base.cpp b/EKF/estimator_base.cpp index d31a510320..866f10b90e 100644 --- a/EKF/estimator_base.cpp +++ b/EKF/estimator_base.cpp @@ -53,7 +53,7 @@ EstimatorBase::~EstimatorBase() } -// read integrated imu data and store to buffer +// Accumulate imu data and store to buffer at desired rate void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang, float *delta_vel) { @@ -132,7 +132,6 @@ void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64 _imu_sample_delayed = _imu_buffer.get_oldest(); } - void EstimatorBase::setMagData(uint64_t time_usec, float *data) { @@ -161,11 +160,9 @@ void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps) return; } - if (time_usec - _time_last_gps > 70000 && gps_is_good(gps)) { - gpsSample gps_sample_new = {}; - gps_sample_new.time_us = time_usec - _params.gps_delay_ms * 1000; + gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000; gps_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2; @@ -260,25 +257,24 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec) _params.mag_delay_ms = 0; _params.baro_delay_ms = 0; - _params.gps_delay_ms = 0; + _params.gps_delay_ms = 200; _params.airspeed_delay_ms = 0; - _params.requiredEph = 10; - _params.requiredEpv = 10; - _params.dax_noise = 0.0f; - _params.day_noise = 0.0f; - _params.daz_noise = 0.0f; - _params.dvx_noise = 0.0f; - _params.dvy_noise = 0.0f; - _params.dvz_noise = 0.0f; - _params.delta_ang_sig = 0.0f; - _params.delta_vel_sig = 0.0f; - _params.delta_pos_sig = 0.0f; - _params.delta_gyro_bias_sig = 0.0f; - _params.delta_gyro_scale_sig = 0.0f; - _params.delta_vel_bias_z_sig = 0.0f; - _params.delta_mag_body_sig = 0.0f; - _params.delta_mag_earth_sig = 0.0f; - _params.delta_wind_sig = 0.0f; + _params.requiredEph = 200; + _params.requiredEpv = 200; + _params.gyro_noise = 1e-3f; + _params.accel_noise = 1e-1f; + _params.gyro_bias_p_noise = 1e-5f; + _params.accel_bias_p_noise = 1e-3f; + _params.gyro_scale_p_noise = 1e-4f; + _params.mag_p_noise = 1e-2f; + _params.wind_vel_p_noise = 0.05f; + + _params.gps_vel_noise = 0.05f; + _params.gps_pos_noise = 1.0f; + _params.baro_noise = 0.1f; + _params.mag_heading_noise = 3e-2f; + _params.mag_declination_deg = 10.0f; + _params.heading_innov_gate = 0.5f; _dt_imu_avg = 0.0f; _imu_time_last = time_usec; @@ -308,6 +304,9 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec) _gps_initialised = false; _gps_speed_valid = false; + _mag_healthy = false; + _in_air = true; // XXX get this flag from the application + _time_last_imu = 0; _time_last_gps = 0; _time_last_mag = 0; @@ -315,6 +314,8 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec) _time_last_range = 0; _time_last_airspeed = 0; + memset(&_fault_status, 0, sizeof(_fault_status)); + } void EstimatorBase::initialiseGPS(struct gps_message *gps) diff --git a/EKF/estimator_base.h b/EKF/estimator_base.h index 55c86d8928..20693f6d1d 100644 --- a/EKF/estimator_base.h +++ b/EKF/estimator_base.h @@ -64,7 +64,7 @@ public: EstimatorBase(); ~EstimatorBase(); - virtual void update() = 0; + virtual bool update() = 0; // set delta angle imu data void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang, float *delta_vel); @@ -111,6 +111,7 @@ protected: Quaternion quat_nominal; Vector3f vel; Vector3f pos; + uint64_t time_us; }; struct imuSample { @@ -118,40 +119,40 @@ protected: Vector3f delta_vel; float delta_ang_dt; float delta_vel_dt; - uint32_t time_us; + uint64_t time_us; }; struct gpsSample { Vector2f pos; float hgt; Vector3f vel; - uint32_t time_us; + uint64_t time_us; }; struct magSample { Vector3f mag; - uint32_t time_us; + uint64_t time_us; }; struct baroSample { float hgt; - uint32_t time_us; + uint64_t time_us; }; struct rangeSample { float rng; - uint32_t time_us; + uint64_t time_us; }; struct airspeedSample { float airspeed; - uint32_t time_us; + uint64_t time_us; }; struct flowSample { Vector2f flowRadXY; Vector2f flowRadXYcomp; - uint32_t time_us; + uint64_t time_us; }; struct { @@ -162,31 +163,32 @@ protected: float requiredEph; float requiredEpv; - float dax_noise; - float day_noise; - float daz_noise; - float dvx_noise; - float dvy_noise; - float dvz_noise; + float gyro_noise; + float accel_noise; - float delta_ang_sig; - float delta_vel_sig; - float delta_pos_sig; - float delta_gyro_bias_sig; - float delta_gyro_scale_sig; - float delta_vel_bias_z_sig; - float delta_mag_body_sig; - float delta_mag_earth_sig; - float delta_wind_sig; + // process noise + float gyro_bias_p_noise; + float accel_bias_p_noise; + float gyro_scale_p_noise; + float mag_p_noise; + float wind_vel_p_noise; + + float gps_vel_noise; + float gps_pos_noise; + float baro_noise; + + float mag_heading_noise; // measurement noise used for simple heading fusion + float mag_declination_deg; // magnetic declination in degrees + float heading_innov_gate; // innovation gate for heading innovation test } _params; - static const uint8_t OBS_BUFFER_LENGTH = 5; - static const uint8_t IMU_BUFFER_LENGTH = 25; + static const uint8_t OBS_BUFFER_LENGTH = 10; + static const uint8_t IMU_BUFFER_LENGTH = 30; static const unsigned FILTER_UPDATE_PERRIOD_MS = 10; float _dt_imu_avg; - uint32_t _imu_time_last; + uint64_t _imu_time_last; imuSample _imu_sample_delayed; imuSample _imu_down_sampled; @@ -200,11 +202,13 @@ protected: airspeedSample _airspeed_sample_delayed; flowSample _flow_sample_delayed; + outputSample _output_delayed; + struct map_projection_reference_s _posRef; float _gps_alt_ref; - uint32_t _imu_ticks; + uint64_t _imu_ticks; bool _imu_updated; bool _start_predict_enabled; @@ -212,6 +216,10 @@ protected: bool _gps_initialised; bool _gps_speed_valid; + bool _mag_healthy; // computed by mag innovation test + + bool _in_air; // indicates if the vehicle is in the air + RingBuffer _imu_buffer; RingBuffer _gps_buffer; RingBuffer _mag_buffer; @@ -228,6 +236,15 @@ protected: uint64_t _time_last_range; uint64_t _time_last_airspeed; + // flags capturing information about severe nummerical problems for various fusions + struct { + bool bad_mag_x:1; + bool bad_mag_y:1; + bool bad_mag_z:1; + bool bad_airspeed:1; + bool bad_sideslip:1; + } _fault_status; + void initialiseVariables(uint64_t timestamp); @@ -246,4 +263,23 @@ public: void printStoredBaro(); void printGps(struct gpsSample *data); void printStoredGps(); + + void copy_quaternion(float *quat) { + for (unsigned i = 0; i < 4; i++) { + quat[i] = _state.quat_nominal(i); + } + } + void copy_velocity(float *vel) { + for (unsigned i = 0; i < 3; i++) { + vel[i] = _state.vel(i); + } + } + void copy_position(float *pos) { + for (unsigned i = 0; i < 3; i++) { + pos[i] = _state.pos(i); + } + } + void copy_timestamp(uint64_t *time_us) { + *time_us = _imu_time_last; + } }; diff --git a/EKF/heading_fusion.cpp b/EKF/heading_fusion.cpp new file mode 100644 index 0000000000..2fa108380a --- /dev/null +++ b/EKF/heading_fusion.cpp @@ -0,0 +1,182 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file heading_fusion.cpp + * Magnetometer heading fusion. + * + * @author Roman Bast + * + */ +#include "ekf.h" +#include + +void Ekf::fuseHeading() +{ + // assign intermediate state variables + float q0 = _state.quat_nominal(0); + float q1 = _state.quat_nominal(1); + float q2 = _state.quat_nominal(2); + float q3 = _state.quat_nominal(3); + + float magX = _mag_sample_delayed.mag(0); + float magY = _mag_sample_delayed.mag(1); + float magZ = _mag_sample_delayed.mag(2); + + float R_mag = _params.mag_heading_noise; + + float t2 = q0*q0; + float t3 = q1*q1; + float t4 = q2*q2; + float t5 = q3*q3; + float t6 = q0*q2*2.0f; + float t7 = q1*q3*2.0f; + float t8 = t6+t7; + float t9 = q0*q3*2.0f; + float t13 = q1*q2*2.0f; + float t10 = t9-t13; + float t11 = t2+t3-t4-t5; + float t12 = magX*t11; + float t14 = magZ*t8; + float t19 = magY*t10; + float t15 = t12+t14-t19; + float t16 = t2-t3+t4-t5; + float t17 = q0*q1*2.0f; + float t24 = q2*q3*2.0f; + float t18 = t17-t24; + float t20 = 1.0f/t15; + float t21 = magY*t16; + float t22 = t9+t13; + float t23 = magX*t22; + float t28 = magZ*t18; + float t25 = t21+t23-t28; + float t29 = t20*t25; + float t26 = tan(t29); + float t27 = 1.0f/(t15*t15); + float t30 = t26*t26; + float t31 = t30+1.0f; + + float H_MAG[3] = {}; + H_MAG[0] = -t31*(t20*(magZ*t16+magY*t18)+t25*t27*(magY*t8+magZ*t10)); + H_MAG[1] = t31*(t20*(magX*t18+magZ*t22)+t25*t27*(magX*t8-magZ*t11)); + H_MAG[2] = t31*(t20*(magX*t16-magY*t22)+t25*t27*(magX*t10+magY*t11)); + + // calculate innovation + matrix::Dcm R_to_earth(_state.quat_nominal); + matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + + float innovation = atan2f(mag_earth_pred(1), mag_earth_pred(0)) - math::radians(_params.mag_declination_deg); + + innovation = math::constrain(innovation, -0.5f, 0.5f); + + float innovation_var = R_mag; + + // calculate innovation variance + float PH[3] = {}; + for (unsigned row = 0; row < 3; row++) { + for (unsigned column = 0; column < 3; column++) { + PH[row] += P[row][column]*H_MAG[column]; + } + innovation_var += H_MAG[row] * PH[row]; + } + + if (innovation_var >= R_mag) { + // variance has increased, no failure + _fault_status.bad_mag_x = false; + _fault_status.bad_mag_y = false; + _fault_status.bad_mag_z = false; + } else { + // our innovation variance has decreased, our calculation is thus badly conditioned + _fault_status.bad_mag_x = true; + _fault_status.bad_mag_y = true; + _fault_status.bad_mag_z = true; + + // we reinitialise the covariance matrix and abort this fusion step + initialiseCovariance(); + return; + } + + float innovation_var_inv = 1 / innovation_var; + + // calculate kalman gain + float Kfusion[_k_num_states] = {}; + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < 3; column++) { + Kfusion[row] += P[row][column] * H_MAG[column]; + } + Kfusion[row] *= innovation_var_inv; + } + + // innovation test ratio + float yaw_test_ratio = sq(innovation) / (sq(math::max(0.01f * (float)_params.heading_innov_gate, 1.0f)) * innovation_var); + + // set the magnetometer unhealthy if the test fails + if (yaw_test_ratio > 1.0f) { + _mag_healthy = false; + + // if we are in air we don't want to fuse the measurement + // we allow to use it when on the ground because the large innovation could be caused + // by interference or a large initial gyro bias + if (_in_air) { + return; + } + } else { + _mag_healthy = true; + } + + _state.ang_error.setZero(); + fuse(Kfusion, innovation); + + // correct the nominal quaternion + Quaternion dq; + dq.from_axis_angle(_state.ang_error); + _state.quat_nominal = dq * _state.quat_nominal; + _state.quat_nominal.normalize(); + + float HP[_k_num_states] = {}; + + for (unsigned column = 0; column < _k_num_states; column++) { + for (unsigned row = 0; row < 3; row++) { + HP[column] += H_MAG[row] * P[row][column]; + } + } + + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < _k_num_states; column++) { + P[row][column] -= Kfusion[row] * HP[column]; + } + } + + makeSymmetrical(); + limitCov(); +} \ No newline at end of file diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp new file mode 100644 index 0000000000..9c8cf53ffb --- /dev/null +++ b/EKF/vel_pos_fusion.cpp @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vel_pos_fusion.cpp + * Function for fusing gps and baro measurements/ + * + * @author Roman Bast + * + */ + +#include "ekf.h" + + void Ekf::fuseVelPosHeight() + { + bool fuse_map[6] = {}; + float innovations[6] = {}; + float R[6] = {}; + float Kfusion[24] = {}; + // calculate innovations + if (_fuse_vel) { + fuse_map[0] = fuse_map[1] = fuse_map[2] = true; + innovations[0] = _state.vel(0) - _gps_sample_delayed.vel(0); + innovations[1] = _state.vel(1) - _gps_sample_delayed.vel(1); + innovations[2] = _state.vel(2) - _gps_sample_delayed.vel(2); + R[0] = _params.gps_vel_noise; + R[1] = _params.gps_vel_noise; + R[2] = _params.gps_vel_noise; + } + + if (_fuse_pos) { + fuse_map[3] = fuse_map[4] = true; + innovations[3] = _state.pos(0) - _gps_sample_delayed.pos(0); + innovations[4] = _state.pos(1) - _gps_sample_delayed.pos(1); + R[3] = _params.gps_pos_noise; + R[4] = _params.gps_pos_noise; + + } + + if (_fuse_height) { + fuse_map[5] = true; + innovations[5] = _state.pos(2) - (-_baro_sample_delayed.hgt); // baro measurement has inversed z axis + R[5] = _params.baro_noise; + } + + // XXX Do checks here + + for (unsigned obs_index = 0; obs_index < 6; obs_index++) { + if (!fuse_map[obs_index]) { + continue; + } + + unsigned state_index = obs_index + 3; // we start with vx and this is the 4. state + + // compute the innovation variance SK = HPH + R + float S = P[state_index][state_index] + R[obs_index]; + S = 1.0f / S; + + // calculate kalman gain K = PHS + for (int row = 0; row < 24; row++) { + Kfusion[row] = P[row][state_index] * S; + } + + // by definition the angle error state is zero at the fusion time + _state.ang_error.setZero(); + + // fuse the observation + fuse(Kfusion, innovations[obs_index]); + + // correct the nominal quaternion + Quaternion dq; + dq.from_axis_angle(_state.ang_error); + _state.quat_nominal = dq * _state.quat_nominal; + _state.quat_nominal.normalize(); + + // update covarinace matrix via Pnew = (I - KH)P + float KHP[_k_num_states][_k_num_states] = {}; + + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < _k_num_states; column++) { + KHP[row][column] = Kfusion[row] * P[state_index][column]; + } + } + + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < _k_num_states; column++) { + P[row][column] = P[row][column] - KHP[row][column]; + } + } + + makeSymmetrical(); + limitCov(); + } + + } + + void Ekf::fuse(float *K, float innovation) + { + for (unsigned i = 0; i < 3; i++) { + _state.ang_error(i) = _state.ang_error(i) - K[i] * innovation; + } + + for (unsigned i = 0; i < 3; i++) { + _state.vel(i) = _state.vel(i) - K[i + 3] * innovation; + } + + for (unsigned i = 0; i < 3; i++) { + _state.pos(i) = _state.pos(i) - K[i + 6] * innovation; + } + + for (unsigned i = 0; i < 3; i++) { + _state.gyro_bias(i) = _state.gyro_bias(i) - K[i + 9] * innovation; + } + + for (unsigned i = 0; i < 3; i++) { + _state.gyro_scale(i) = _state.gyro_scale(i) - K[i + 12] * innovation; + } + + _state.accel_z_bias -= K[15] * innovation; + + for (unsigned i = 0; i < 3; i++) { + _state.mag_I(i) = _state.mag_I(i) - K[i + 16] * innovation; + } + + for (unsigned i = 0; i < 3; i++) { + _state.mag_B(i) = _state.mag_B(i) - K[i + 19] * innovation; + } + + for (unsigned i = 0; i < 2; i++) { + _state.wind_vel(i) = _state.wind_vel(i) - K[i + 22] * innovation; + } + }