fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 03:17:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #86 from CarlOlsson/small_airspeed_fix

Browse Source 
Small airspeed fix
This commit is contained in:
Lorenz Meier
2016-04-05 16:38:37 -07:00
parent f35e78643c eee6f1048f
commit 6b3ad03419
1 changed files with 2 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/airspeed_fusion.cpp
+2 -3
View File
@@ -70,10 +70,10 @@ 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 / (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;
@@ -180,5 +180,4 @@ void Ekf::fuseAirspeed()
makeSymmetrical();
limitCov();
}
// Do we want to force and limit the covariance matrx even if v_tas_pred < X ?
}