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

Improve on flash memory usage

Browse Source
This commit is contained in:
kamilritz
2020-01-02 19:26:57 +11:00
committed by Paul Riseborough
parent 459b76f9fd
commit 92ba618f57
13 changed files with 75 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/airspeed_fusion.cpp
+2 -6
View File
@@ -176,7 +176,7 @@ void Ekf::fuseAirspeed()
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP
// then calculate P - KHP
matrix::SquareMatrix<float, _k_num_states> KHP {};
matrix::SquareMatrix<float, _k_num_states> KHP;
float KH[5];
for (unsigned row = 0; row < _k_num_states; row++) {
@@ -219,11 +219,7 @@ void Ekf::fuseAirspeed()
// only apply covariance and state corrections if healthy
if (healthy) {
// apply the covariance corrections
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);
}
}
P = P - KHP;
// correct the covariance matrix for gross errors
fixCovarianceErrors();