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

ekf_helper: fixed resetting height using range finder

Browse Source 
- take range sensor offset wrt to IMU into account
- use projection to earth Z axis

Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman
2017-06-15 09:10:43 +02:00
committed by ChristophTobler
parent 3778f0921a
commit 1b92c9b5b3
1 changed files with 5 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+5 -1
View File
@@ -158,8 +158,12 @@ void Ekf::resetHeight()
rangeSample range_newest = _range_buffer.get_newest();
if (_time_last_imu - range_newest.time_us < 2 * RNG_MAX_INTERVAL) {
// correct the range data for position offset relative to the IMU
Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
range_newest.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2;
// calculate the new vertical position using range sensor
float new_pos_down = _hgt_sensor_offset - range_newest.rng;
float new_pos_down = _hgt_sensor_offset - range_newest.rng * _R_rng_to_earth_2_2;
// update the state and assoicated variance
_state.pos(2) = new_pos_down;