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

ekf2: move height fusion code to separate source file

Browse Source
This commit is contained in:
bresch 2021-12-06 15:26:58 +01:00
parent ba1526896d
commit 59a0aebaf2
5 changed files with 129 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/ekf2/CMakeLists.txt
View File

@ -57,6 +57,7 @@ px4_add_module(
EKF/gps_control.cpp
EKF/gps_fusion.cpp
EKF/gps_yaw_fusion.cpp
EKF/height_fusion.cpp
EKF/imu_down_sampler.cpp
EKF/mag_control.cpp
EKF/mag_fusion.cpp

1
src/modules/ekf2/EKF/CMakeLists.txt
View File

@ -45,6 +45,7 @@ add_library(ecl_EKF
gps_control.cpp
gps_fusion.cpp
gps_yaw_fusion.cpp
height_fusion.cpp
imu_down_sampler.cpp
mag_control.cpp
mag_fusion.cpp

68
src/modules/ekf2/EKF/control.cpp
View File

@ -955,76 +955,16 @@ void Ekf::controlHeightFusion()
if (fuse_height) {
if (_control_status.flags.baro_hgt) {
Vector2f baro_hgt_innov_gate;
Vector3f baro_hgt_obs_var;
// vertical position innovation - baro measurement has opposite sign to earth z axis
const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();
_baro_hgt_innov(2) = _state.pos(2) + unbiased_baro - _baro_hgt_offset;
// observation variance - user parameter defined
baro_hgt_obs_var(2) = sq(fmaxf(_params.baro_noise, 0.01f));
// innovation gate size
baro_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
// Compensate for positive static pressure transients (negative vertical position innovations)
// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
const float deadzone_start = 0.0f;
const float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
if (_control_status.flags.gnd_effect) {
if (_baro_hgt_innov(2) < -deadzone_start) {
if (_baro_hgt_innov(2) <= -deadzone_end) {
_baro_hgt_innov(2) += deadzone_end;
} else {
_baro_hgt_innov(2) = -deadzone_start;
}
}
}
// fuse height information
fuseVerticalPosition(_baro_hgt_innov, baro_hgt_innov_gate,
baro_hgt_obs_var, _baro_hgt_innov_var, _baro_hgt_test_ratio);
fuseBaroHgt();
} else if (_control_status.flags.gps_hgt) {
Vector2f gps_hgt_innov_gate;
Vector3f gps_hgt_obs_var;
// vertical position innovation - gps measurement has opposite sign to earth z axis
_gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
gps_hgt_obs_var(2) = getGpsHeightVariance();
// innovation gate size
gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
// fuse height information
fuseVerticalPosition(_gps_pos_innov, gps_hgt_innov_gate,
gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
fuseGpsHgt();
} else if (_control_status.flags.rng_hgt) {
Vector2f rng_hgt_innov_gate;
Vector3f rng_hgt_obs_var;
// use range finder with tilt correction
_rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(),
_params.rng_gnd_clearance)) - _hgt_sensor_offset;
// observation variance - user parameter defined
rng_hgt_obs_var(2) = fmaxf(sq(_params.range_noise)
+ sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
// innovation gate size
rng_hgt_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f);
// fuse height information
fuseVerticalPosition(_rng_hgt_innov, rng_hgt_innov_gate,
rng_hgt_obs_var, _rng_hgt_innov_var, _rng_hgt_test_ratio);
fuseRngHgt();
} else if (_control_status.flags.ev_hgt) {
Vector2f ev_hgt_innov_gate;
Vector3f ev_hgt_obs_var;
// calculate the innovation assuming the external vision observation is in local NED frame
_ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2);
// observation variance - defined externally
ev_hgt_obs_var(2) = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f));
// innovation gate size
ev_hgt_innov_gate(1) = fmaxf(_params.ev_pos_innov_gate, 1.0f);
// fuse height information
fuseVerticalPosition(_ev_pos_innov, ev_hgt_innov_gate,
ev_hgt_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
fuseEvHgt();
}
}
}

5
src/modules/ekf2/EKF/ekf.h
View File

@ -625,6 +625,11 @@ private:
// fuse body frame drag specific forces for multi-rotor wind estimation
void fuseDrag();
void fuseBaroHgt();
void fuseGpsHgt();
void fuseRngHgt();
void fuseEvHgt();
// fuse single velocity and position measurement
void fuseVelPosHeight(const float innov, const float innov_var, const int obs_index);

118
src/modules/ekf2/EKF/height_fusion.cpp Normal file
View File

@ -0,0 +1,118 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file height_fusion.cpp
* Function for fusing height (range, baro, GNSS alt, ...) measurements
*/
#include "ekf.h"
void Ekf::fuseBaroHgt()
{
Vector2f baro_hgt_innov_gate;
Vector3f baro_hgt_obs_var;
// vertical position innovation - baro measurement has opposite sign to earth z axis
const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();
_baro_hgt_innov(2) = _state.pos(2) + unbiased_baro - _baro_hgt_offset;
// observation variance - user parameter defined
baro_hgt_obs_var(2) = sq(fmaxf(_params.baro_noise, 0.01f));
// innovation gate size
baro_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
// Compensate for positive static pressure transients (negative vertical position innovations)
// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
const float deadzone_start = 0.0f;
const float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
if (_control_status.flags.gnd_effect) {
if (_baro_hgt_innov(2) < -deadzone_start) {
if (_baro_hgt_innov(2) <= -deadzone_end) {
_baro_hgt_innov(2) += deadzone_end;
} else {
_baro_hgt_innov(2) = -deadzone_start;
}
}
}
fuseVerticalPosition(_baro_hgt_innov, baro_hgt_innov_gate,
baro_hgt_obs_var, _baro_hgt_innov_var, _baro_hgt_test_ratio);
}
void Ekf::fuseGpsHgt()
{
Vector2f gps_hgt_innov_gate;
Vector3f gps_hgt_obs_var;
// vertical position innovation - gps measurement has opposite sign to earth z axis
_gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
gps_hgt_obs_var(2) = getGpsHeightVariance();
// innovation gate size
gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
fuseVerticalPosition(_gps_pos_innov, gps_hgt_innov_gate,
gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
}
void Ekf::fuseRngHgt()
{
Vector2f rng_hgt_innov_gate;
Vector3f rng_hgt_obs_var;
// use range finder with tilt correction
_rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(),
_params.rng_gnd_clearance)) - _hgt_sensor_offset;
// observation variance - user parameter defined
rng_hgt_obs_var(2) = fmaxf(sq(_params.range_noise)
+ sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
// innovation gate size
rng_hgt_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f);
fuseVerticalPosition(_rng_hgt_innov, rng_hgt_innov_gate,
rng_hgt_obs_var, _rng_hgt_innov_var, _rng_hgt_test_ratio);
}
void Ekf::fuseEvHgt()
{
Vector2f ev_hgt_innov_gate;
Vector3f ev_hgt_obs_var;
// calculate the innovation assuming the external vision observation is in local NED frame
_ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2);
// observation variance - defined externally
ev_hgt_obs_var(2) = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f));
// innovation gate size
ev_hgt_innov_gate(1) = fmaxf(_params.ev_pos_innov_gate, 1.0f);
fuseVerticalPosition(_ev_pos_innov, ev_hgt_innov_gate,
ev_hgt_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
}