From 616725410d457e1ef8709fa7f656901f222ab1c2 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Sun, 3 Apr 2016 21:01:29 +0200 Subject: [PATCH 1/3] updated calculation --- EKF/airspeed_fusion.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index d630fe6546..3b5d90c1fd 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -73,7 +73,7 @@ void Ekf::fuseAirspeed() 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[0] = 1 / v_tas_pred; SH_TAS[1] = (SH_TAS[0] * (2 * ve - 2 * vwe)) / 2.0f; SH_TAS[2] = (SH_TAS[0] * (2 * vn - 2 * vwn)) / 2.0f; From 3ad5b52aeaf68f31ea73bf7d814545026a22ce73 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Sun, 3 Apr 2016 20:20:00 +0200 Subject: [PATCH 2/3] removed comment --- EKF/airspeed_fusion.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 3b5d90c1fd..18b394a2dc 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -182,5 +182,4 @@ void Ekf::fuseAirspeed() makeSymmetrical(); limitCov(); } - // Do we want to force and limit the covariance matrx even if v_tas_pred < X ? } \ No newline at end of file From eee6f1048fbfee155ef3d331d40cb43786729835 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Sun, 3 Apr 2016 20:08:51 +0200 Subject: [PATCH 3/3] Changed vtaspred threshold --- EKF/airspeed_fusion.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 18b394a2dc..1d15cd15d9 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -70,7 +70,7 @@ void Ekf::fuseAirspeed() 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) { + if (v_tas_pred > 1.0f) { // Calculate the observation jacobian // intermediate variable from algebraic optimisation SH_TAS[0] = 1 / v_tas_pred;