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

EKF: Enable fallback to baro alt when using range finder for height

Browse Source
This commit is contained in:
Paul Riseborough
2016-03-13 21:17:51 +11:00
parent 370f643f42
commit c58ab3e256
5 changed files with 52 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+2 -2
View File
@@ -159,8 +159,8 @@ void Ekf::controlFusionModes()
// Handle the case where we have rejected height measurements for an extended period
// This excessive vibration levels can cause this so a reset gives the filter a chance to recover
// After 10 seconds without aiding we reset to the height measurement provided the data is fresh
if ((_time_last_imu - _time_last_hgt_fuse > 10e6) && (_time_last_imu - _time_last_baro < 5e5)) {
// After 10 seconds without aiding we reset to the height measurement
if (_time_last_imu - _time_last_hgt_fuse > 10e6) {
// Reset vertical position and velocity states to the last measurement
resetHeight();
}
EKF/ekf.cpp
+37 -16
View File
@@ -40,6 +40,7 @@
*/
#include "ekf.h"
#include "mathlib.h"
Ekf::Ekf():
_filter_initialised(false),
@@ -78,7 +79,8 @@ Ekf::Ekf():
_terrain_vpos(0.0f),
_hagl_innov(0.0f),
_hagl_innov_var(0.0f),
_time_last_hagl_fuse(0)
_time_last_hagl_fuse(0),
_baro_hgt_offset(0.0f)
{
_control_status = {};
_control_status_prev = {};
@@ -101,7 +103,7 @@ Ekf::Ekf():
_mag_sum = {};
_delVel_sum = {};
_flow_gyro_bias = {};
_imu_del_ang_of = {};
_imu_del_ang_of = {};
}
Ekf::~Ekf()
@@ -209,7 +211,7 @@ bool Ekf::update()
// determine if range finder data has fallen behind the fusin time horizon fuse it if we are
// not tilted too much to use it
if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
&& (_R_prev(2, 2) > 0.7071f)) {
&& (_R_prev(2, 2) > 0.7071f)) {
// if we have range data we always try to estimate terrain height
_fuse_hagl_data = true;
@@ -217,20 +219,34 @@ bool Ekf::update()
if (_control_status.flags.rng_hgt) {
_fuse_height = true;
}
}
// If we are supposed to be using range finder data as the primary height sensor and haven't received it for a while
// and are on the ground, then set the measurement to the expected on ground value
if ((_time_last_imu - _time_last_hgt_fuse) > 5e5 && _control_status.flags.rng_hgt && !_in_air) {
_range_sample_delayed.rng = _params.rng_gnd_clearance;
_range_sample_delayed.time_us = _imu_sample_delayed.time_us;
} else if ((_time_last_imu - _time_last_hgt_fuse) > 5e5 && _control_status.flags.rng_hgt) {
// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
// and are on the ground, then synthesise a measurement at the expected on ground value
if (!_in_air) {
_range_sample_delayed.rng = _params.rng_gnd_clearance;
_range_sample_delayed.time_us = _imu_sample_delayed.time_us;
} else {
// if this happens in the air, fuse the baro measurement to constrain drift
// use the baro for this update only
_control_status.flags.baro_hgt = true;
_control_status.flags.rng_hgt = false;
}
_fuse_height = true;
}
// determine if baro data has fallen behind the fuson time horizon and fuse it in the main filter if enabled
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)
&& _control_status.flags.baro_hgt) {
_fuse_height = true;
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
if (_control_status.flags.baro_hgt) {
_fuse_height = true;
} else {
// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
_baro_hgt_offset += 0.02f * math::constrain((_baro_sample_delayed.hgt + _state.pos(2) - _baro_hgt_offset), -5.0f, 5.0f);
}
}
// If we are using GPS aiding and data has fallen behind the fusion time horizon then fuse it
@@ -242,15 +258,15 @@ bool Ekf::update()
// If we are using optical flow aiding and data has fallen behind the fusion time horizon, then fuse it
if (_flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
&& _control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow) < 2e5
&& (_R_prev(2, 2) > 0.7071f)) {
&& _control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow) < 2e5
&& (_R_prev(2, 2) > 0.7071f)) {
_fuse_flow = true;
}
// if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift
// Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz
if (!_control_status.flags.gps && !_control_status.flags.opt_flow
&& ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
&& ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
_fuse_pos = true;
_gps_sample_delayed.pos(0) = _last_known_posNE(0);
_gps_sample_delayed.pos(1) = _last_known_posNE(1);
@@ -375,9 +391,14 @@ bool Ekf::initialiseFilter(void)
// calculate the initial magnetic field and yaw alignment
resetMagHeading(mag_init);
// calculate the averaged height reading
// calculate the averaged height reading to calulate the height of the origin
_hgt_at_alignment = _hgt_sum / (float)_hgt_counter;
// if we are not using the baro height as the primary source, then calculate an offset relative to the origin
// so it can be used as a backup
baroSample baro_newest = _baro_buffer.get_newest();
_baro_hgt_offset = baro_newest.hgt;
// set the velocity to the GPS measurement (by definition, the initial position and height is at the origin)
resetVelocity();
EKF/ekf.h
+2
View File
@@ -216,6 +216,8 @@ private:
uint64_t _time_last_hagl_fuse; // last system time in usec that the hagl measurement failed it's checks
bool _terrain_initialised; // true when the terrain estimator has been intialised
float _baro_hgt_offset; // number of metres the baro height origin is above the local NED origin (m)
// update the real time complementary filter states. This includes the prediction
// and the correction step
void calculateOutputStates();
EKF/ekf_helper.cpp
+9
View File
@@ -96,6 +96,11 @@ void Ekf::resetHeight()
} else {
// TODO: reset to last known range based estimate
}
// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
baroSample baro_newest = _baro_buffer.get_newest();
_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
} else if (_control_status.flags.baro_hgt) {
// initialize vertical position with newest baro measurement
baroSample baro_newest = _baro_buffer.get_newest();
@@ -106,6 +111,10 @@ void Ekf::resetHeight()
} else {
// TODO: reset to last known baro based estimate
}
// the baro height offset should be zero if baro is our primary height source
_baro_hgt_offset = 0.0f;
} else {
// TODO: reset to GPS height
}
EKF/vel_pos_fusion.cpp
+2 -2
View File
@@ -110,7 +110,7 @@ void Ekf::fuseVelPosHeight()
if (_control_status.flags.baro_hgt) {
fuse_map[5] = true;
// vertical position innovation - baro measurement has opposite sign to earth z axis
_vel_pos_innov[5] = _state.pos(2) - (_hgt_at_alignment - _baro_sample_delayed.hgt);
_vel_pos_innov[5] = _state.pos(2) - (_hgt_at_alignment - _baro_sample_delayed.hgt + _baro_hgt_offset);
// observation variance - user parameter defined
R[5] = fmaxf(_params.baro_noise, 0.01f);
R[5] = R[5] * R[5];
@@ -120,7 +120,7 @@ void Ekf::fuseVelPosHeight()
} else if (_control_status.flags.rng_hgt && (_R_prev(2, 2) > 0.7071f)) {
fuse_map[5] = true;
// use range finder with tilt correction
_vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng *_R_prev(2, 2),
_vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_prev(2, 2),
_params.rng_gnd_clearance));
// observation variance - user parameter defined
R[5] = fmaxf(_params.range_noise, 0.01f);