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

Added vision delay compensation to LPE. (#5585)

Browse Source
This commit is contained in:
James Goppert
2016-10-03 02:28:46 -04:00
committed by GitHub
parent 2f6abf26a3
commit 7c2798269c
5 changed files with 50 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/sensors/vision.cpp
+8 -1
View File
@@ -76,9 +76,16 @@ void BlockLocalPositionEstimator::visionCorrect()
R(Y_vision_y, Y_vision_y) = _vision_xy_stddev.get() * _vision_xy_stddev.get();
R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get();
// vision delayed x
uint8_t i_hist = 0;
if (getDelayPeriods(_vision_delay.get(), &i_hist) < 0) { return; }
Vector<float, n_x> x0 = _xDelay.get(i_hist);
// residual
Matrix<float, n_y_vision, n_y_vision> S_I = inv<float, n_y_vision>((C * _P * C.transpose()) + R);
Matrix<float, n_y_vision, 1> r = y - C * _x;
Matrix<float, n_y_vision, 1> r = y - C * x0;
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);