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

EKF: Add arbitrary pitch offset for range sensor

Browse Source
This commit is contained in:
Paul Riseborough
2017-02-22 14:12:05 +01:00
committed by Roman
parent d9688fae7d
commit cf9c8de167
6 changed files with 23 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/terrain_estimator.cpp
+5 -5
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 && _R_to_earth(2,2) > 0.7071f) {
if ((_time_last_imu - latest_measurement.time_us) < 2e5 && _R_rng_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 * _R_to_earth(2, 2);
_terrain_vpos = _state.pos(2) + latest_measurement.rng * _R_rng_to_earth_2_2;
// initialise state variance to variance of measurement
_terrain_var = sq(_params.range_noise);
// success
@@ -88,9 +88,9 @@ void Ekf::predictHagl()
void Ekf::fuseHagl()
{
// If the vehicle is excessively tilted, do not try to fuse range finder observations
if (_R_to_earth(2, 2) > 0.7071f) {
if (_R_rng_to_earth_2_2 > 0.7071f) {
// get a height above ground measurement from the range finder assuming a flat earth
float meas_hagl = _range_sample_delayed.rng * _R_to_earth(2, 2);
float meas_hagl = _range_sample_delayed.rng * _R_rng_to_earth_2_2;
// predict the hagl from the vehicle position and terrain height
float pred_hagl = _terrain_vpos - _state.pos(2);
@@ -99,7 +99,7 @@ void Ekf::fuseHagl()
_hagl_innov = pred_hagl - meas_hagl;
// calculate the observation variance adding the variance of the vehicles own height uncertainty and factoring in the effect of tilt on measurement error
float obs_variance = fmaxf(P[9][9], 0.0f) + sq(_params.range_noise / _R_to_earth(2, 2));
float obs_variance = fmaxf(P[9][9], 0.0f) + sq(_params.range_noise / _R_rng_to_earth_2_2);
// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
_hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);