From a9894d7e8c5adfe515efac6854345ea03c78a3e1 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 24 Jul 2016 09:23:50 +1000 Subject: [PATCH 1/7] EKF: improve efficiency of airspeed fusion Don't compute array entries where the result will be zero --- EKF/airspeed_fusion.cpp | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index fbdd4417dd..2ad385f387 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -150,18 +150,25 @@ void Ekf::fuseAirspeed() // Airspeed measurement sample has passed check so record it _time_last_arsp_fuse = _time_last_imu; - // update covariance matrix via Pnew = (I - KH)P = P - KHP + // apply covariance correction via P_new = (I -K*H)*P + // first calculate expression for KHP + // then calculate P - KHP for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { // Here it will be a lot of zeros, should optimize that... - KH[row][column] = Kfusion[row] * H_TAS[column]; - } + KH[row][4] = Kfusion[row] * H_TAS[4]; + KH[row][5] = Kfusion[row] * H_TAS[5]; + KH[row][6] = Kfusion[row] * H_TAS[6]; + KH[row][22] = Kfusion[row] * H_TAS[22]; + KH[row][23] = Kfusion[row] * H_TAS[23]; } for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned column = 0; column < _k_num_states; column++) { - for (unsigned i = 0; i < _k_num_states; i++) { // Check if this is correct matrix multiplication! - KHP[row][column] += KH[row][i] * P[i][column]; - } + float tmp = KH[row][4] * P[4][column]; + tmp += KH[row][5] * P[5][column]; + tmp += KH[row][6] * P[6][column]; + tmp += KH[row][22] * P[22][column]; + tmp += KH[row][23] * P[23][column]; + KHP[row][column] = tmp; } } From eff9af549f19e508becf4451d587cc0eac56842d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 24 Jul 2016 21:14:29 +1000 Subject: [PATCH 2/7] scripts: derive conversion from polar to cartesian wind state covariance --- matlab/scripts/Inertial Nav EKF/polar2cart_cov.m | 8 ++++++++ 1 file changed, 8 insertions(+) create mode 100644 matlab/scripts/Inertial Nav EKF/polar2cart_cov.m diff --git a/matlab/scripts/Inertial Nav EKF/polar2cart_cov.m b/matlab/scripts/Inertial Nav EKF/polar2cart_cov.m new file mode 100644 index 0000000000..bfb0bc6108 --- /dev/null +++ b/matlab/scripts/Inertial Nav EKF/polar2cart_cov.m @@ -0,0 +1,8 @@ +clear all; +syms spd yaw real; +syms R_spd R_yaw real; +vx = spd*cos(yaw); +vy = spd*sin(yaw); +Tpc = jacobian([vx;vy],[spd;yaw]); +R_polar = [R_spd 0;0 R_yaw]; +R_cartesian = Tpc*R_polar*Tpc'; \ No newline at end of file From 6dff2df28e2c60a27122c4f54c1896bc1018e699 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 24 Jul 2016 21:27:53 +1000 Subject: [PATCH 3/7] EKF: Add method to reset wind state covariances Set variances in polar coordinates and rotate into cartesian frame --- EKF/covariance.cpp | 32 ++++++++++++++++++++++++++++++++ EKF/ekf.h | 3 +++ 2 files changed, 35 insertions(+) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index c994c1242b..b6ad09d511 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -794,3 +794,35 @@ void Ekf::resetMagCovariance() P[rc_index][rc_index] = sq(_params.mag_noise); } } + +void Ekf::resetWindCovariance() +{ + // set the wind covariance terms to zero + zeroRows(P,22,23); + zeroCols(P,22,23); + + // calculate the wind speed and bearing + float spd = sqrtf(sq(_state.wind_vel(0))+sq(_state.wind_vel(1))); + float yaw = atan2f(_state.wind_vel(1),_state.wind_vel(0)); + + // calculate the uncertainty in wind speed and direction using the uncertainty in airspeed and sideslip angle + // used to calculate the initial wind speed + float R_spd = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f)); + float R_yaw = sq(0.1745f); + + // calculate the variance and covariance terms for the wind states + float cos_yaw = cosf(yaw); + float sin_yaw = sinf(yaw); + float cos_yaw_2 = sq(cos_yaw); + float sin_yaw_2 = sq(sin_yaw); + float sin_cos_yaw = sin_yaw*cos_yaw; + float spd_2 = sq(spd); + P[22][22] = R_yaw*spd_2*sin_yaw_2 + R_spd*cos_yaw_2; + P[22][23] = - R_yaw*sin_cos_yaw*spd_2 + R_spd*sin_cos_yaw; + P[23][22] = P[22][23]; + P[23][23] = R_yaw*spd_2*cos_yaw_2 + R_spd*sin_yaw_2; + + // Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed + P[22][22] += P[4][4]; + P[23][23] += P[5][5]; +} diff --git a/EKF/ekf.h b/EKF/ekf.h index 167ed9cffa..0142a4f0c2 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -430,4 +430,7 @@ private: // perform a limited reset of the magnetic field state covariances void resetMagCovariance(); + // perform a limited reset of the wind state covariances + void resetWindCovariance(); + }; From 75bec44b947addf7fb6ab4525fe6f4c807c98af4 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 31 Jul 2016 14:41:48 +1000 Subject: [PATCH 4/7] EKF: add function to initialise wind states --- EKF/airspeed_fusion.cpp | 14 ++++++++++++++ EKF/ekf.h | 3 +++ 2 files changed, 17 insertions(+) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 2ad385f387..ae72399199 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -215,3 +215,17 @@ void Ekf::get_wind_velocity(float *wind) wind[0] = _state.wind_vel(0); wind[1] = _state.wind_vel(1); } + +/* + * Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip +*/ +void Ekf::resetWindStates() +{ + // get euler yaw angle + matrix::Euler euler321(_state.quat_nominal); + float euler_yaw = euler321(2); + + // estimate wind assuming zero sideslip + _state.wind_vel(0) = _state.vel(0) - _airspeed_sample_delayed.true_airspeed * cosf(euler_yaw); + _state.wind_vel(1) = _state.vel(1) - _airspeed_sample_delayed.true_airspeed * sinf(euler_yaw); +} diff --git a/EKF/ekf.h b/EKF/ekf.h index 0142a4f0c2..be8c65595b 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -433,4 +433,7 @@ private: // perform a limited reset of the wind state covariances void resetWindCovariance(); + // perform a reset of the wind states + void resetWindStates(); + }; From d1d56d9e5e94f24d3719a4dd8b85ce65495cac76 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 31 Jul 2016 15:00:25 +1000 Subject: [PATCH 5/7] EKF: Add logic to to control wind state estimation --- EKF/control.cpp | 29 ++++++++++++++++++----------- 1 file changed, 18 insertions(+), 11 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index bcba216819..a717f5b169 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -738,22 +738,29 @@ void Ekf::controlRangeFinderFusion() void Ekf::controlAirDataFusion() { - // TODO This is just to get the logic inside but we will only start fusion once we tested this again - //if (_tas_data_ready) { - if (false) { - fuseAirspeed(); - - } - - // control airspeed fusion - TODO move to a function - // if the airspeed measurements have timed out for 10 seconds we declare the wind estimate to be invalid + // control activation and initialisation/reset of wind states + // wind states are required to do airspeed fusion if (_time_last_imu - _time_last_arsp_fuse > 10e6 || _time_last_arsp_fuse == 0) { + // if the airspeed measurements have timed out for 10 seconds we declare the wind estimate to be invalid _control_status.flags.wind = false; - } else { - _control_status.flags.wind = true; + } else if (_tas_data_ready) { + // if we have airspeed data to fuse and winds states are inactive, then + // they need to be activated and the corresponding states and covariances reset + if (!_control_status.flags.wind) { + _control_status.flags.wind = true; + resetWindStates(); + resetWindCovariance(); + } + + } + + // always fuse airspeed if available and the wind states are active + if (_tas_data_ready && _control_status.flags.wind) { + fuseAirspeed(); } + } void Ekf::controlMagFusion() From 51c8715638245f64b7b01e41414df8c0fb5cc82b Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Tue, 9 Aug 2016 15:46:16 +0200 Subject: [PATCH 6/7] fixed issue with airspeed never fused --- EKF/control.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index a717f5b169..a0556872ae 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -744,7 +744,9 @@ void Ekf::controlAirDataFusion() // if the airspeed measurements have timed out for 10 seconds we declare the wind estimate to be invalid _control_status.flags.wind = false; - } else if (_tas_data_ready) { + } + + if (_tas_data_ready) { // if we have airspeed data to fuse and winds states are inactive, then // they need to be activated and the corresponding states and covariances reset if (!_control_status.flags.wind) { @@ -753,14 +755,9 @@ void Ekf::controlAirDataFusion() resetWindCovariance(); } - } - - // always fuse airspeed if available and the wind states are active - if (_tas_data_ready && _control_status.flags.wind) { fuseAirspeed(); } - } void Ekf::controlMagFusion() From 8d60da14420c5882c096e3c5dcb2800616a06066 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Tue, 9 Aug 2016 15:46:33 +0200 Subject: [PATCH 7/7] removed old wind initialization --- EKF/covariance.cpp | 44 -------------------------------------------- 1 file changed, 44 deletions(-) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index b6ad09d511..73802b4220 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -563,50 +563,6 @@ void Ekf::predictCovariance() // Don't do covariance prediction on wind states unless we are using them if (_control_status.flags.wind) { - // Check if we have just transitioned to using wind states and set the variances accordingly - if (!_control_status_prev.flags.wind) { - // simple initialisation of wind states: calculate wind component along the forward axis - // of the plane. - - matrix::Euler euler(_output_new.quat_nominal); - float heading = euler(2); - - // ground speed component in the xy plane projected onto the directon the plane is heading to - float ground_speed_xy_nose = _output_new.vel(0) * cosf(heading) + _output_new.vel(1) * sinf(heading); - airspeedSample tmp = _airspeed_buffer.get_newest(); - float airspeed = tmp.true_airspeed; - - // check if the calculation is well conditioned: - // our airspeed measurement is at least as hight as our down velocity and the plane is moving forward - if (airspeed > fabsf(_output_new.vel(2)) && ground_speed_xy_nose > 0) { - - float ground_speed = sqrtf(_output_new.vel(0) * _output_new.vel(0) + _output_new.vel(1) * _output_new.vel(1) + _output_new.vel(2) * _output_new.vel(2)); - - // wind magnitude in the direction the plane is - float wind_magnitude = ground_speed_xy_nose - sqrtf(airspeed * airspeed - _output_new.vel(2) * _output_new.vel(2)); - - // determine direction of wind - float wind_sign = 1; - if (airspeed < ground_speed) { - // wind is in nose direction - wind_sign = 1; - } else { - wind_sign = -1; - } - - _state.wind_vel(0) = cosf(heading) * wind_magnitude * wind_sign; - _state.wind_vel(1) = sinf(heading) * wind_magnitude * wind_sign; - - } else { - // calculation is badly conditioned, just set wind states to zero - _state.wind_vel.setZero(); - } - - // initialise diagonal of covariance matrix for the wind velocity states - for (uint8_t index = 22; index <= 23; index++) { - P[index][index] = sq(5.0f); - } - } // calculate variances and upper diagonal covariances for wind states nextP[0][22] = P[0][22] + P[1][22]*SF[9] + P[2][22]*SF[11] + P[3][22]*SF[10] + P[10][22]*SF[14] + P[11][22]*SF[15] + P[12][22]*SPP[10];