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

local_position_estimator: move to WQ and delete unused SubscriptionPollable

Browse Source
This commit is contained in:
Daniel Agar
2019-11-22 16:02:54 -05:00
parent db69ff0a6e
commit f271efaf62
19 changed files with 372 additions and 816 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/sensors/baro.cpp
+3 -3
View File
@@ -68,7 +68,7 @@ void BlockLocalPositionEstimator::baroCorrect()
// residual
Matrix<float, n_y_baro, n_y_baro> S_I =
inv<float, n_y_baro>((C * _P * C.transpose()) + R);
inv<float, n_y_baro>((C * m_P * C.transpose()) + R);
Vector<float, n_y_baro> r = y - (C * _x);
// fault detection
@@ -87,10 +87,10 @@ void BlockLocalPositionEstimator::baroCorrect()
}
// kalman filter correction always
Matrix<float, n_x, n_y_baro> K = _P * C.transpose() * S_I;
Matrix<float, n_x, n_y_baro> K = m_P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
m_P -= K * C * m_P;
}
void BlockLocalPositionEstimator::baroCheckTimeout()