dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

terrain estimator: initialise with projection 

Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman 2016-09-20 14:09:42 +02:00
parent cc05db4985
commit c6e1d97176
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/terrain_estimator.cpp
View File

@ -47,9 +47,9 @@ bool Ekf::initHagl()
// get most recent range measurement from buffer
rangeSample latest_measurement = _range_buffer.get_newest();
if ((_time_last_imu - latest_measurement.time_us) < 2e5) {
if ((_time_last_imu - latest_measurement.time_us) < 2e5 && _R_to_earth(2,2) > 0.7071f) {
// if we have a fresh measurement, use it to initialise the terrain estimator
_terrain_vpos = _state.pos(2) + latest_measurement.rng;
_terrain_vpos = _state.pos(2) + latest_measurement.rng * _R_to_earth(2, 2);
// initialise state variance to variance of measurement
_terrain_var = sq(_params.range_noise);
// success