Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Airspeed: Airspeed Calibration computational efficiency improvement

Browse Source
This commit is contained in:
Paul Riseborough 2013-07-20 14:27:53 +10:00 committed by Andrew Tridgell
parent 0c6725f289
commit d1805f4349
1 changed files with 7 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Airspeed/Airspeed_Calibration.cpp
View File

@ -89,11 +89,13 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg)
// force symmetry on the covariance matrix - necessary due to rounding
// errors
// Implementation will also need a further check to prevent diagonal
// terms becoming negative due to rounding errors
// This step can be made more efficient by excluding diagonal terms
// (would reduce processing by 1/3)
P = (P + P.transpose()) * 0.5f; // [1 x 1] * ( [3 x 3] + [3 x 3])
// Implementation may also need a further check to prevent diagonal
float P12 = 0.5f * (P.a.y + P.b.x);
float P13 = 0.5f * (P.a.z + P.c.x);
float P23 = 0.5f * (P.b.z + P.c.y);
P.a.y = P.b.x = P12;
P.a.z = P.c.x = P12;
P.b.z = P.c.y = P13;
return state.z;
}