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

lpe : use per-measurement variance for vision

Browse Source
This commit is contained in:
Kabir Mohammed
2017-02-15 21:48:33 +01:00
committed by Lorenz Meier
parent 48e7b94162
commit 85df00d2da
1 changed files with 16 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/sensors/vision.cpp
+16 -3
View File
@@ -72,9 +72,22 @@ void BlockLocalPositionEstimator::visionCorrect()
// noise matrix
Matrix<float, n_y_vision, n_y_vision> R;
R.setZero();
R(Y_vision_x, Y_vision_x) = _vision_xy_stddev.get() * _vision_xy_stddev.get();
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();
if (_sub_vision_pos.get().eph > _vision_xy_stddev.get()) {
R(Y_vision_x, Y_vision_x) = _sub_vision_pos.get().eph * _sub_vision_pos.get().eph;
R(Y_vision_y, Y_vision_y) = _sub_vision_pos.get().eph * _sub_vision_pos.get().eph;
} else {
R(Y_vision_x, Y_vision_x) = _vision_xy_stddev.get() * _vision_xy_stddev.get();
R(Y_vision_y, Y_vision_y) = _vision_xy_stddev.get() * _vision_xy_stddev.get();
}
if (_sub_vision_pos.get().epv > _vision_z_stddev.get()) {
R(Y_vision_z, Y_vision_z) = _sub_vision_pos.get().epv * _sub_vision_pos.get().epv;
} else {
R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get();
}
// vision delayed x
uint8_t i_hist = 0;