From 0fb1e0578ba0b5dc84067426fdb45696eed6554b Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Fri, 11 Mar 2016 12:42:16 +0100 Subject: [PATCH 01/12] added function for fusing airspeed --- CMakeLists.txt | 1 + EKF/CMakeLists.txt | 1 + EKF/airspeed_fusion.cpp | 186 ++++++++++++++++++++++++++++++++++++++++ EKF/ekf.cpp | 7 +- 4 files changed, 189 insertions(+), 6 deletions(-) create mode 100644 EKF/airspeed_fusion.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 62f2a90f43..60f49dc144 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -55,6 +55,7 @@ px4_add_module( EKF/vel_pos_fusion.cpp EKF/mag_fusion.cpp EKF/gps_checks.cpp + EKF/airspeed_fusion.cpp EKF/optflow_fusion.cpp EKF/control.cpp EKF/terrain_estimator.cpp diff --git a/EKF/CMakeLists.txt b/EKF/CMakeLists.txt index eee1af9037..6400204045 100644 --- a/EKF/CMakeLists.txt +++ b/EKF/CMakeLists.txt @@ -64,6 +64,7 @@ set(SRCS vel_pos_fusion.cpp mag_fusion.cpp gps_checks.cpp + airspeed_fusion.cpp control.cpp geo.cpp mathlib.cpp diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp new file mode 100644 index 0000000000..d630fe6546 --- /dev/null +++ b/EKF/airspeed_fusion.cpp @@ -0,0 +1,186 @@ +/**************************************************************************** + * + * 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 airspeed_fusion.cpp + * airspeed fusion methods. + * + * @author Carl Olsson + * @author Roman Bast + * @author Paul Riseborough + * + */ +#include "ekf.h" +#include "mathlib.h" + +void Ekf::fuseAirspeed() +{ + // Initialize variables + float vn; // Velocity in north direction + float ve; // Velocity in east direction + float vd; // Velocity in downwards direction + float vwn; // Wind speed in north direction + float vwe; // Wind speed in east direction + float v_tas_pred; // Predicted measurement + float EAS2TAS = 1.0f; // Where can I get this from? + float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(EAS2TAS, 0.9f, 10.0f)); // Variance for true airspeed measurement - (m/sec)^2 + float SH_TAS[3] = {}; // Varialbe used to optimise calculations of measurement jacobian + float H_TAS[24] = {}; // Observation Jacobian + float SK_TAS[2] = {}; // Varialbe used to optimise calculations of the Kalman gain vector + float Kfusion[24] = {}; // Kalman gain vector + + // Copy required states to local variable names + vn = _state.vel(0); + ve = _state.vel(1); + vd = _state.vel(2); + vwn = _state.wind_vel(0); + vwe = _state.wind_vel(1); + + // Calculate the predicted airspeed + v_tas_pred = sqrtf((ve - vwe) * (ve - vwe) + (vn - vwn) * (vn - vwn) + vd * vd); + + // Perform fusion of True Airspeed measurement + if (v_tas_pred > 3.0f) { + // Calculate the observation jacobian + // intermediate variable from algebraic optimisation + SH_TAS[0] = 1 / (sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); + SH_TAS[1] = (SH_TAS[0] * (2 * ve - 2 * vwe)) / 2.0f; + SH_TAS[2] = (SH_TAS[0] * (2 * vn - 2 * vwn)) / 2.0f; + + for (uint8_t i = 0; i < _k_num_states; i++) { H_TAS[i] = 0.0f; } + + H_TAS[3] = SH_TAS[2]; + H_TAS[4] = SH_TAS[1]; + H_TAS[5] = vd * SH_TAS[0]; + H_TAS[22] = -SH_TAS[2]; + H_TAS[23] = -SH_TAS[1]; + + // We don't want to update the innovation variance if the calculation is ill conditioned + float _airspeed_innov_var_temp = (R_TAS + SH_TAS[2] * (P[3][3] * SH_TAS[2] + P[4][3] * SH_TAS[1] - P[22][3] * SH_TAS[2] - P[23][3] * SH_TAS[1] + P[5][3] * vd *SH_TAS[0]) + SH_TAS[1] * (P[3][4] * SH_TAS[2] + P[4][4] * SH_TAS[1] - P[22][4] * SH_TAS[2] - P[23][4] * SH_TAS[1] + P[5][4] * vd *SH_TAS[0]) - SH_TAS[2] * (P[3][22] * SH_TAS[2] + P[4][22] * SH_TAS[1] - P[22][22] * SH_TAS[2] - P[23][22] * SH_TAS[1] + P[5][22] * vd *SH_TAS[0]) - SH_TAS[1] * (P[3][23] * SH_TAS[2] + P[4][23] * SH_TAS[1] - P[22][23] * SH_TAS[2] - P[23][23] * SH_TAS[1] + P[5][23] * vd *SH_TAS[0]) + vd *SH_TAS[0] * (P[3][5] * SH_TAS[2] + P[4][5] * SH_TAS[1] - P[22][5] * SH_TAS[2] - P[23][5] * SH_TAS[1] + P[5][5] * vd *SH_TAS[0])); + if (_airspeed_innov_var_temp >= R_TAS) { // Check for badly conditioned calculation + SK_TAS[0] = 1.0f / _airspeed_innov_var_temp; + } else { // Reset the estimator + initialiseCovariance(); + return; + } + SK_TAS[1] = SH_TAS[1]; + + Kfusion[0] = SK_TAS[0]*(P[0][3]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][4]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][5]*vd*SH_TAS[0]); + Kfusion[1] = SK_TAS[0]*(P[1][3]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][4]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][5]*vd*SH_TAS[0]); + Kfusion[2] = SK_TAS[0]*(P[2][3]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][4]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][5]*vd*SH_TAS[0]); + Kfusion[3] = SK_TAS[0]*(P[3][3]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][4]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][5]*vd*SH_TAS[0]); + Kfusion[4] = SK_TAS[0]*(P[4][3]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][4]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][5]*vd*SH_TAS[0]); + Kfusion[5] = SK_TAS[0]*(P[5][3]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][4]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][5]*vd*SH_TAS[0]); + Kfusion[6] = SK_TAS[0]*(P[6][3]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][4]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][5]*vd*SH_TAS[0]); + Kfusion[7] = SK_TAS[0]*(P[7][3]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][4]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][5]*vd*SH_TAS[0]); + Kfusion[8] = SK_TAS[0]*(P[8][3]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][4]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][5]*vd*SH_TAS[0]); + Kfusion[9] = SK_TAS[0]*(P[9][3]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][4]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][5]*vd*SH_TAS[0]); + Kfusion[10] = SK_TAS[0]*(P[10][3]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][4]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][5]*vd*SH_TAS[0]); + Kfusion[11] = SK_TAS[0]*(P[11][3]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][4]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][5]*vd*SH_TAS[0]); + Kfusion[12] = SK_TAS[0]*(P[12][3]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][4]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][5]*vd*SH_TAS[0]); + Kfusion[13] = SK_TAS[0]*(P[13][3]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][4]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][5]*vd*SH_TAS[0]); + Kfusion[14] = SK_TAS[0]*(P[14][3]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][4]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][5]*vd*SH_TAS[0]); + Kfusion[15] = SK_TAS[0]*(P[15][3]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][4]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][5]*vd*SH_TAS[0]); + Kfusion[22] = SK_TAS[0]*(P[22][3]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][4]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][5]*vd*SH_TAS[0]); + Kfusion[23] = SK_TAS[0]*(P[23][3]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][4]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][5]*vd*SH_TAS[0]); + + // Only update the magnetometer states if we are airborne and using 3D mag fusion + if(_control_status.flags.mag_3D && _control_status.flags.in_air){ + Kfusion[16] = SK_TAS[0]*(P[16][3]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][4]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][5]*vd*SH_TAS[0]); + Kfusion[17] = SK_TAS[0]*(P[17][3]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][4]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][5]*vd*SH_TAS[0]); + Kfusion[18] = SK_TAS[0]*(P[18][3]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][4]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][5]*vd*SH_TAS[0]); + Kfusion[19] = SK_TAS[0]*(P[19][3]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][4]*SK_TAS[1] - P[19][23]*SK_TAS[1] + P[19][5]*vd*SH_TAS[0]); + Kfusion[20] = SK_TAS[0]*(P[20][3]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][4]*SK_TAS[1] - P[20][23]*SK_TAS[1] + P[20][5]*vd*SH_TAS[0]); + Kfusion[21] = SK_TAS[0]*(P[21][3]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][4]*SK_TAS[1] - P[21][23]*SK_TAS[1] + P[21][5]*vd*SH_TAS[0]); + } + else{ + for (int i = 16; i <= 21; i++) + { + Kfusion[i] = 0.0f; + } + } + + + // calculate measurement innovation + _airspeed_innov = v_tas_pred - _airspeed_sample_delayed.airspeed; // This is TAS, maybe we should indicate that in some way + + // Calculate the innovation variance + _airspeed_innov_var = 1.0f / SK_TAS[0]; + + // Compute the ratio of innovation to gate size + _tas_test_ratio = sq(_airspeed_innov) / (sq(fmaxf(_params.tas_innov_gate, 1.0f)) * _airspeed_innov_var); + + if (_tas_test_ratio < 1.0f) + { + // by definition the angle error state is zero at the fusion time + _state.ang_error.setZero(); + + // Fuse airspeed measurement + fuse(Kfusion, _airspeed_innov); //Why calculate angle error when it is always zero? + + // 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 covariance matrix via Pnew = (I - KH)P = P - KHP + float KH[_k_num_states][_k_num_states] = {}; + 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++) { // Here it will be a lot of zeros, should optimize that... + KH[row][column] = Kfusion[row] * H_TAS[column]; + } + } + + 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]; + } + } + } + + 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(); + } + // Do we want to force and limit the covariance matrx even if v_tas_pred < X ? +} \ No newline at end of file diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index cdfe4cc01b..fe2bc52719 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -587,9 +587,4 @@ void Ekf::calculateOutputStates() _delta_vel_corr = (_state.vel - _output_sample_delayed.vel) * imu_new.delta_vel_dt; _vel_corr = (_state.pos - _output_sample_delayed.pos); -} - -void Ekf::fuseAirspeed() -{ - -} +} \ No newline at end of file From 92abf93c8fcae46672d19f9a0b76008c743f8027 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Fri, 11 Mar 2016 12:47:12 +0100 Subject: [PATCH 02/12] adopted common.h --- EKF/common.h | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/EKF/common.h b/EKF/common.h index 8fd9d2652b..b1466f88f4 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -112,7 +112,7 @@ struct rangeSample { }; struct airspeedSample { - float airspeed; // airspeed measurement in m/s + float airspeed; // true airspeed measurement in m/s uint64_t time_us; // timestamp in microseconds }; @@ -195,6 +195,10 @@ struct parameters { int mag_declination_source; // bitmask used to control the handling of declination data int mag_fusion_type; // integer used to specify the type of magnetometer fusion used + // airspeed fusion + float tas_innov_gate; // True Airspeed Innovation consistency gate size in standard deciation [WHAT SHALL THIS VALUE BE?] + float eas_noise; // EAS measurement noise standard deviation used for airspeed fusion [m/s] + // range finder fusion float range_noise; // observation noise for range finder measurements (m) float range_innov_gate; // range finder fusion innovation consistency gate size (STD) @@ -265,6 +269,10 @@ struct parameters { mag_declination_source = 3; mag_fusion_type = 0; + // airspeed fusion + float tas_innov_gate = 3.0f; // [CHECK THIS VALUE] + float eas_noise = 1.4f; + // range finder fusion range_noise = 0.1f; range_innov_gate = 5.0f; From 0918fa04c740f0ebb4ee384c157325259ba7b4b2 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Fri, 11 Mar 2016 12:49:15 +0100 Subject: [PATCH 03/12] adopted ekf.cpp --- EKF/ekf.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index fe2bc52719..f432f77c41 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -93,6 +93,8 @@ Ekf::Ekf(): memset(_flow_innov, 0, sizeof(_flow_innov)); memset(_vel_pos_innov_var, 0, sizeof(_vel_pos_innov_var)); memset(_mag_innov_var, 0, sizeof(_mag_innov_var)); + memset(_airspeed_innov, 0 sizeof(_airspeed_innov)); + memset(_airspeed_innov_var, 0 sizeof(_airspeed_innov_var)); memset(_flow_innov_var, 0, sizeof(_flow_innov_var)); _delta_angle_corr.setZero(); _delta_vel_corr.setZero(); From 6bb2f7638c261384506b16b293db732ad1439a8f Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Fri, 11 Mar 2016 12:52:38 +0100 Subject: [PATCH 04/12] adopted ekf.h --- EKF/ekf.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/EKF/ekf.h b/EKF/ekf.h index 51a01a9be2..b98f9769d3 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -73,6 +73,12 @@ public: // gets the innovation variances of the earth magnetic field measurements void get_mag_innov_var(float mag_innov_var[3]); + // gets the innovations of airspeed measurement + void get_airspeed_innov(float *airspeed_innov); + + // gets the innovation variance of the airspeed measurement + void get_airspeed_innov_var(float *airspeed_innov_var); + // gets the innovation variance of the heading measurement void get_heading_innov_var(float *heading_innov_var); @@ -155,6 +161,9 @@ private: float _mag_innov[3]; // earth magnetic field innovations float _mag_innov_var[3]; // earth magnetic field innovation variance + float _airspeed_innov; // airspeed measurement innovation + float _airspeed_innov_var; // airspeed measurement innovation variance + float _heading_innov; // heading measurement innovation float _heading_innov_var; // heading measurement innovation variance From f8878d41dc14ee27225cae800eaf250480278d08 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Fri, 11 Mar 2016 12:54:33 +0100 Subject: [PATCH 05/12] adopted ekf_helper.cpp --- EKF/ekf_helper.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index f74fb7392d..371366a731 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -267,6 +267,12 @@ void Ekf::get_mag_innov(float mag_innov[3]) memcpy(mag_innov, _mag_innov, 3 * sizeof(float)); } +// gets the innovations of the airspeed measnurement +void Ekf::get_airspeed_innov(float *airspeed_innov) +{ + memcpy(airspeed_innov,&_airspeed_innov, sizeof(float)); +} + // gets the innovations of the heading measurement void Ekf::get_heading_innov(float *heading_innov) { @@ -286,6 +292,12 @@ void Ekf::get_mag_innov_var(float mag_innov_var[3]) memcpy(mag_innov_var, _mag_innov_var, sizeof(float) * 3); } +// gest the innovation variance of the airspeed measurement +void Ekf::get_airspeed_innov_var(float *airspeed_innov_var) +{ + memcpy(airspeed_innov_var, &_airspeed_innov_var, sizeof(float)); +} + // gets the innovation variance of the heading measurement void Ekf::get_heading_innov_var(float *heading_innov_var) { From d440c883b55c0ab74831dd184d7118dd9b858c53 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Fri, 11 Mar 2016 12:55:33 +0100 Subject: [PATCH 06/12] adopted ekf_interface.cpp --- EKF/estimator_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 373a703f80..2185a2c9be 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -206,11 +206,11 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float *data) return; } - if (time_usec > _time_last_airspeed) { + if (time_usec - _time_last_airspeed > 1000000) { //Limit the airspeed update rate to 1 Hz for now airspeedSample airspeed_sample_new; airspeed_sample_new.airspeed = *data; airspeed_sample_new.time_us = time_usec - _params.airspeed_delay_ms * 1000; - airspeed_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2; + airspeed_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2; //typo PeRRiod _time_last_airspeed = time_usec; _airspeed_buffer.push(airspeed_sample_new); From 81fc086b769757dc5a339fcd0291a95eb93bc85f Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Fri, 11 Mar 2016 12:57:14 +0100 Subject: [PATCH 07/12] adopted ekf_interface.h --- EKF/estimator_interface.h | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 2e5a3aca1a..17b453fb75 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -63,6 +63,9 @@ public: // gets the innovations of the earth magnetic field measurements virtual void get_mag_innov(float mag_innov[3]) = 0; + // gets the innovation of airspeed measurement + virtual void get_airspeed_innov(float *airspeed_innov) = 0; + // gets the innovations of the heading measurement virtual void get_heading_innov(float *heading_innov) = 0; @@ -73,6 +76,9 @@ public: // gets the innovation variances of the earth magnetic field measurements virtual void get_mag_innov_var(float mag_innov_var[3]) = 0; + // gets the innovation variance of the airspeed measurement + virtual void get_airspeed_innov_var(float *get_airspeed_innov_var) = 0; + // gets the innovation variance of the heading measurement virtual void get_heading_innov_var(float *heading_innov_var) = 0; @@ -229,8 +235,8 @@ 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 + float _tas_test_ratio; // tas innovation consistency check ratio // data buffer instances RingBuffer _imu_buffer; From 4301e1105b5515aa45235089788cbbac2b777fbd Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Fri, 11 Mar 2016 14:37:04 +0100 Subject: [PATCH 08/12] added commas --- EKF/ekf.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index f432f77c41..6a52adf094 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -93,8 +93,8 @@ Ekf::Ekf(): memset(_flow_innov, 0, sizeof(_flow_innov)); memset(_vel_pos_innov_var, 0, sizeof(_vel_pos_innov_var)); memset(_mag_innov_var, 0, sizeof(_mag_innov_var)); - memset(_airspeed_innov, 0 sizeof(_airspeed_innov)); - memset(_airspeed_innov_var, 0 sizeof(_airspeed_innov_var)); + memset(_airspeed_innov, 0, sizeof(_airspeed_innov)); + memset(_airspeed_innov_var, 0, sizeof(_airspeed_innov_var)); memset(_flow_innov_var, 0, sizeof(_flow_innov_var)); _delta_angle_corr.setZero(); _delta_vel_corr.setZero(); From f990d997901e8b8bd7aaf88103486213ccc768c3 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Fri, 11 Mar 2016 14:40:33 +0100 Subject: [PATCH 09/12] fixed bug --- EKF/common.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index b1466f88f4..95ce9fb7a4 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -270,8 +270,8 @@ struct parameters { mag_fusion_type = 0; // airspeed fusion - float tas_innov_gate = 3.0f; // [CHECK THIS VALUE] - float eas_noise = 1.4f; + tas_innov_gate = 3.0f; // [CHECK THIS VALUE] + eas_noise = 1.4f; // range finder fusion range_noise = 0.1f; From f5a9afd27833ecbf92af84c35230f7c7b471d6d6 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Sun, 13 Mar 2016 13:41:23 +0100 Subject: [PATCH 10/12] moved init --- EKF/ekf.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 6a52adf094..c891c45181 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -57,6 +57,8 @@ Ekf::Ekf(): _time_last_hgt_fuse(0), _time_last_of_fuse(0), _last_disarmed_posD(0.0f), + _airspeed_innov(0.0f), + _airspeed_innov_var(0.0f), _heading_innov(0.0f), _heading_innov_var(0.0f), _delta_time_of(0.0f), @@ -93,8 +95,6 @@ Ekf::Ekf(): memset(_flow_innov, 0, sizeof(_flow_innov)); memset(_vel_pos_innov_var, 0, sizeof(_vel_pos_innov_var)); memset(_mag_innov_var, 0, sizeof(_mag_innov_var)); - memset(_airspeed_innov, 0, sizeof(_airspeed_innov)); - memset(_airspeed_innov_var, 0, sizeof(_airspeed_innov_var)); memset(_flow_innov_var, 0, sizeof(_flow_innov_var)); _delta_angle_corr.setZero(); _delta_vel_corr.setZero(); From 1b7115dec9d91d4b9868b640cb2fc389b3f23b5f Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 30 Mar 2016 16:57:17 +0200 Subject: [PATCH 11/12] increased airspeed fusion rate to 12.5Hz --- EKF/estimator_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 2185a2c9be..c744b9b208 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -206,7 +206,7 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float *data) return; } - if (time_usec - _time_last_airspeed > 1000000) { //Limit the airspeed update rate to 1 Hz for now + if (time_usec - _time_last_airspeed > 80000) { airspeedSample airspeed_sample_new; airspeed_sample_new.airspeed = *data; airspeed_sample_new.time_us = time_usec - _params.airspeed_delay_ms * 1000; From 27b894540ed9f4a87e02f1958a396664b3c7d6bd Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 30 Mar 2016 16:57:40 +0200 Subject: [PATCH 12/12] put airspeed fusion logic but don't actually call it yet --- EKF/ekf.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index c891c45181..07ccb422c2 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -305,6 +305,11 @@ bool Ekf::update() _last_known_posNE(1) = _state.pos(1); _fuse_flow = false; } + + // TODO This is just to get the logic inside but we will only start fusion once we tested this again + if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed) && false) { + fuseAirspeed(); + } } // the output observer always runs