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

Merge pull request #102 from priseborough/pr-ekf2HeightReset

Browse Source 
EKF: Improve height reset behaviour
This commit is contained in:
Paul Riseborough
2016-04-23 10:04:13 +10:00
parent 13814681d7 4fcbfb5d42
commit 42635903cd
7 changed files with 233 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/RingBuffer.h
+24
View File
@@ -160,6 +160,30 @@ public:
return _buffer[index];
}
// return data at the specified index
data_type get_from_index(unsigned index)
{
if (index >= _size) {
index = _size-1;
}
return _buffer[index];
}
// push data to the specified index
void push_to_index(unsigned index, data_type sample)
{
if (index >= _size) {
index = _size-1;
}
_buffer[index] = sample;
}
// return the length of the buffer
unsigned get_length()
{
return _size;
}
private:
data_type *_buffer;
unsigned _head, _tail, _size;
EKF/common.h
+1 -1
View File
@@ -267,7 +267,7 @@ struct parameters {
baro_innov_gate = 5.0f;
posNE_innov_gate = 5.0f;
vel_innov_gate = 5.0f;
hgt_reset_lim = 5.0f;
hgt_reset_lim = 0.0f;
// magnetometer fusion
mag_heading_noise = 3.0e-1f;
EKF/control.cpp
+120 -19
View File
@@ -156,6 +156,10 @@ void Ekf::controlFusionModes()
// Reset states to the last GPS measurement
resetPosition();
resetVelocity();
// Reset the timeout counters
_time_last_pos_fuse = _time_last_imu;
_time_last_vel_fuse = _time_last_imu;
}
}
}
@@ -166,7 +170,24 @@ void Ekf::controlFusionModes()
* measurement source, continue using it after the reset and declare the current
* source failed if we have switched.
*/
// check for inertial sensing errors as evidenced by the vertical innovations having the same sign and not stale
bool bad_vert_accel = (_control_status.flags.baro_hgt && // we can only run this check if vertical position and velocity observations are indepedant
(_vel_pos_innov[5] * _vel_pos_innov[2] > 0.0f) && // vertical position and velocity sensors are in agreement
((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) && // vertical position data is fresh
((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL) && // vertical velocity data is freshs
_vel_pos_test_ratio[2] > 1.0f && // vertical velocty innovations have failed innovation consistency checks
_vel_pos_test_ratio[5] > 1.0f); // vertical position innovations have failed innovation consistency checks
// record time of last bad vert accel
if (bad_vert_accel) {
_time_bad_vert_accel = _time_last_imu;
}
if ((P[8][8] > sq(_params.hgt_reset_lim)) && ((_time_last_imu - _time_last_hgt_fuse) > 5e6)) {
// boolean that indicates we will do a height reset
bool reset_height = false;
// handle the case where we are using baro for height
if (_control_status.flags.baro_hgt) {
// check if GPS height is available
@@ -176,17 +197,45 @@ void Ekf::controlFusionModes()
baroSample baro_init = _baro_buffer.get_newest();
bool baro_hgt_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
// use the gps if it is accurate or there is no baro data available
if (gps_hgt_available && (gps_hgt_accurate || !baro_hgt_available)) {
// declare the baro as unhealthy
// check for inertial sensing errors in the last 10 seconds
bool prev_bad_vert_accel = (_time_last_imu - _time_bad_vert_accel < 10E6);
// reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data
bool reset_to_gps = gps_hgt_available && gps_hgt_accurate && !_gps_hgt_faulty && !prev_bad_vert_accel;
// reset to GPS if GPS data is available and there is no Baro data
reset_to_gps = reset_to_gps || (gps_hgt_available && !baro_hgt_available);
// reset to Baro if we are not doing a GPS reset and baro data is available
bool reset_to_baro = !reset_to_gps && baro_hgt_available;
if (reset_to_gps) {
// set height sensor health
_baro_hgt_faulty = true;
// set the height mode to the GPS
_gps_hgt_faulty = false;
// declare the GPS height healthy
_gps_hgt_faulty = false;
// reset the height mode
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = true;
_control_status.flags.rng_hgt = false;
// adjust the height offset so we can use the GPS
_hgt_sensor_offset = _state.pos(2) + gps_init.hgt - _gps_alt_ref;
printf("EKF baro hgt timeout - switching to gps\n");
// request a reset
reset_height = true;
printf("EKF baro hgt timeout - reset to GPS\n");
} else if (reset_to_baro){
// set height sensor health
_baro_hgt_faulty = false;
// reset the height mode
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
// request a reset
reset_height = true;
printf("EKF baro hgt timeout - reset to baro\n");
} else {
// we have nothing we can reset to
// deny a reset
reset_height = false;
}
}
@@ -202,15 +251,39 @@ void Ekf::controlFusionModes()
float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P[8][8]) * sq(_params.baro_innov_gate);
// if baro data is consistent and fresh or GPS height is unavailable or inaccurate, we switch to baro for height
if ((baro_data_consistent && baro_data_fresh) || !gps_hgt_available || !gps_hgt_accurate) {
// declare the GPS height unhealthy
// if baro data is acceptable and GPS data is inaccurate, reset height to baro
bool reset_to_baro = baro_data_consistent && baro_data_fresh && !_baro_hgt_faulty && !gps_hgt_accurate;
// if GPS height is unavailable and baro data is available, reset height to baro
reset_to_baro = reset_to_baro || (!gps_hgt_available && baro_data_fresh);
// if we cannot switch to baro and GPs data is available, reset height to GPS
bool reset_to_gps = !reset_to_baro && gps_hgt_available;
if (reset_to_baro) {
// set height sensor health
_gps_hgt_faulty = true;
// set the height mode to the baro
_baro_hgt_faulty = false;
// reset the height mode
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
printf("EKF gps hgt timeout - switching to baro\n");
// request a reset
reset_height = true;
printf("EKF gps hgt timeout - reset to baro\n");
} else if (reset_to_gps) {
// set height sensor health
_gps_hgt_faulty = false;
// reset the height mode
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = true;
_control_status.flags.rng_hgt = false;
// request a reset
reset_height = true;
printf("EKF gps hgt timeout - reset to GPS\n");
} else {
// we have nothing to reset to
reset_height = false;
}
}
@@ -225,22 +298,50 @@ void Ekf::controlFusionModes()
// check if baro data is consistent
float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
bool baro_data_consistent = sq(baro_innov) < (sq(_params.baro_noise) + P[8][8]) * sq(_params.baro_innov_gate);
// switch to baro if necessary or preferable
bool switch_to_baro = (!rng_data_available && baro_data_available) || (baro_data_consistent && baro_data_available);
if (switch_to_baro) {
// declare the range finder height unhealthy
// reset to baro if data is available and we have no range data
bool reset_to_baro = !rng_data_available && baro_data_available;
// reset to baro if data is acceptable
reset_to_baro = reset_to_baro || (baro_data_consistent && baro_data_available && !_baro_hgt_faulty);
// reset to range data if it is available and we cannot switch to baro
bool reset_to_rng = !reset_to_baro && rng_data_available;
if (reset_to_baro) {
// set height sensor health
_rng_hgt_faulty = true;
// set the height mode to the baro
_baro_hgt_faulty = false;
// reset the height mode
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
printf("EKF rng hgt timeout - switching to baro\n");
// request a reset
reset_height = true;
printf("EKF rng hgt timeout - reset to baro\n");
} else if (reset_to_rng) {
// set height sensor health
_rng_hgt_faulty = false;
// reset the height mode
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = true;
// request a reset
reset_height = true;
printf("EKF rng hgt timeout - reset to rng hgt\n");
} else {
// we have nothing to reset to
reset_height = false;
}
}
// Reset vertical position and velocity states to the last measurement
resetHeight();
if (reset_height) {
resetHeight();
// Reset the timout timer
_time_last_hgt_fuse = _time_last_imu;
}
}
// handle the case when we are relying on optical flow fusion and lose it
EKF/ekf.cpp
+23 -16
View File
@@ -74,7 +74,7 @@ Ekf::Ekf():
_last_gps_origin_time_us(0),
_gps_alt_ref(0.0f),
_hgt_counter(0),
_hgt_filt_state(0.0f),
_rng_filt_state(0.0f),
_mag_counter(0),
_time_last_mag(0),
_hgt_sensor_offset(0.0f),
@@ -85,7 +85,12 @@ Ekf::Ekf():
_baro_hgt_faulty(false),
_gps_hgt_faulty(false),
_rng_hgt_faulty(false),
_baro_hgt_offset(0.0f)
_baro_hgt_offset(0.0f),
_vert_pos_reset_delta(0.0f),
_time_vert_pos_reset(0),
_vert_vel_reset_delta(0.0f),
_time_vert_vel_reset(0),
_time_bad_vert_accel(0)
{
_state = {};
_last_known_posNE.setZero();
@@ -384,12 +389,12 @@ bool Ekf::initialiseFilter(void)
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = true;
_hgt_filt_state = _range_sample_delayed.rng;
_rng_filt_state = _range_sample_delayed.rng;
_hgt_counter = 1;
} else if (_hgt_counter != 0) {
// increment the sample count and apply a LPF to the measurement
_hgt_counter ++;
_hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _range_sample_delayed.rng;
_rng_filt_state = 0.9f * _rng_filt_state + 0.1f * _range_sample_delayed.rng;
}
}
@@ -402,12 +407,12 @@ bool Ekf::initialiseFilter(void)
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_hgt_filt_state = _baro_sample_delayed.hgt;
_baro_hgt_offset = _baro_sample_delayed.hgt;
_hgt_counter = 1;
} else if (_hgt_counter != 0) {
// increment the sample count and apply a LPF to the measurement
_hgt_counter ++;
_hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _baro_sample_delayed.hgt;
_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
}
}
@@ -465,17 +470,12 @@ bool Ekf::initialiseFilter(void)
// calculate the initial magnetic field and yaw alignment
resetMagHeading(mag_init);
// calculate the averaged height reading to calulate the height of the origin
_hgt_sensor_offset = _hgt_filt_state;
// 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
if (!_control_status.flags.baro_hgt) {
// if we are using the range finder as the primary source, then calculate the baro height at origin so we can use baro as a backup
// so it can be used as a backup ad set the initial height using the range finder
if (_control_status.flags.rng_hgt) {
baroSample baro_newest = _baro_buffer.get_newest();
_baro_hgt_offset = baro_newest.hgt - _hgt_sensor_offset;
} else {
_baro_hgt_offset = 0.0f;
_baro_hgt_offset = baro_newest.hgt;
_state.pos(2) = -math::max(_rng_filt_state * _R_to_earth(2, 2),_params.rng_gnd_clearance);
}
// initialise the state covariance matrix
@@ -484,6 +484,13 @@ bool Ekf::initialiseFilter(void)
// initialise the terrain estimator
initHagl();
// reset the essential fusion timeout counters
_time_last_hgt_fuse = _time_last_imu;
_time_last_pos_fuse = _time_last_imu;
_time_last_vel_fuse = _time_last_imu;
_time_last_hagl_fuse = _time_last_imu;
_time_last_of_fuse = _time_last_imu;
return true;
}
}
EKF/ekf.h
+14 -4
View File
@@ -126,6 +126,9 @@ public:
// get GPS check status
void get_gps_check_status(uint16_t *_gps_check_fail_status);
// return the amount the local vertical position changed in the last height reset and the time of the reset
void get_vert_pos_reset(float *delta, uint64_t *time_us) {*delta = _vert_pos_reset_delta; *time_us = _time_vert_pos_reset;}
private:
static const uint8_t _k_num_states = 24;
@@ -169,7 +172,7 @@ private:
float _mag_innov_var[3]; // earth magnetic field innovation variance
float _airspeed_innov; // airspeed measurement innovation
float _airspeed_innov_var; // airspeed measurement innovation variance
float _airspeed_innov_var; // airspeed measurement innovation variance
float _heading_innov; // heading measurement innovation
float _heading_innov_var; // heading measurement innovation variance
@@ -208,12 +211,12 @@ private:
// Variables used to initialise the filter states
uint32_t _hgt_counter; // number of height samples taken
float _hgt_filt_state; // filtered height measurement
float _rng_filt_state; // filtered height measurement
uint32_t _mag_counter; // number of magnetometer samples taken
uint64_t _time_last_mag; // measurement time of last magnetomter sample
Vector3f _mag_filt_state; // filtered magnetometer measurement
Vector3f _delVel_sum; // summed delta velocity
float _hgt_sensor_offset; // height that needs to be subtracted from the primary height sensor so that it reads zero height at the origin (m)
float _hgt_sensor_offset; // set as necessary if desired to maintain the same height after a height reset (m)
gps_check_fail_status_u _gps_check_fail_status;
@@ -231,7 +234,14 @@ private:
bool _rng_hgt_faulty; // true if valid rnage finder height data is unavailable for use
int _primary_hgt_source; // priary source of height data set at initialisation
float _baro_hgt_offset; // number of metres the baro height origin is above the local NED origin (m)
float _baro_hgt_offset; // baro height reading at the local NED origin (m)
float _vert_pos_reset_delta; // increase in vertical position state at the last reset(m)
uint64_t _time_vert_pos_reset; // last system time in usec that the vertical position state was reset
float _vert_vel_reset_delta; // increase in vertical position velocity at the last reset(m)
uint64_t _time_vert_vel_reset; // last system time in usec that the vertical velocity state was reset
// imu fault status
uint64_t _time_bad_vert_accel; // last time a bad vertical accel was detected (usec)
// update the real time complementary filter states. This includes the prediction
// and the correction step
EKF/ekf_helper.cpp
+48 -1
View File
@@ -90,12 +90,23 @@ void Ekf::resetHeight()
// Get the most recent GPS data
gpsSample gps_newest = _gps_buffer.get_newest();
// store the current vertical position and velocity for reference so we can calculate and publish the reset amount
float old_vert_pos = _state.pos(2);
bool vert_pos_reset = false;
float old_vert_vel = _state.vel(2);
bool vert_vel_reset = false;
// reset the vertical position
if (_control_status.flags.rng_hgt) {
rangeSample range_newest = _range_buffer.get_newest();
if (_time_last_imu - range_newest.time_us < 2 * RNG_MAX_INTERVAL) {
_state.pos(2) = _hgt_sensor_offset - range_newest.rng;
// calculate the new vertical position using range sensor
float new_pos_down = _hgt_sensor_offset - range_newest.rng;
// update the state and assoicated variance
_state.pos(2) = new_pos_down;
P[8][8] = sq(_params.range_noise);
vert_pos_reset = true;
} else {
// TODO: reset to last known range based estimate
@@ -112,6 +123,7 @@ void Ekf::resetHeight()
if (_time_last_imu - baro_newest.time_us < 2 * BARO_MAX_INTERVAL) {
_state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset;
P[8][8] = sq(_params.baro_noise);
vert_pos_reset = true;
} else {
// TODO: reset to last known baro based estimate
@@ -122,6 +134,7 @@ void Ekf::resetHeight()
if (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL) {
_state.pos(2) = _hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref;
P[8][8] = sq(gps_newest.hacc);
vert_pos_reset = true;
} else {
// TODO: reset to last known gps based estimate
@@ -140,6 +153,40 @@ void Ekf::resetHeight()
P[5][5] = fminf(sq(_state.vel(2)),1000.0f);
_state.vel(2) = 0.0f;
}
vert_vel_reset = true;
// store the reset amount and time to be published
if (vert_pos_reset) {
_vert_pos_reset_delta = _state.pos(2) - old_vert_pos;
_time_vert_pos_reset = _time_last_imu;
}
if (vert_vel_reset) {
_vert_vel_reset_delta = _state.vel(2) - old_vert_vel;
_time_vert_vel_reset = _time_last_imu;
}
// add the reset amount to the output observer states
_output_new.pos(2) += _vert_pos_reset_delta;
_output_new.vel(2) += _vert_vel_reset_delta;
// add the reset amount to the output observer buffered data
outputSample output_states;
unsigned output_length = _output_buffer.get_length();
for (unsigned i=0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
if (vert_pos_reset) {
output_states.pos(2) += _vert_pos_reset_delta;
}
if (vert_vel_reset) {
output_states.vel(2) += _vert_vel_reset_delta;
}
_output_buffer.push_to_index(i,output_states);
}
}
EKF/estimator_interface.h
+3
View File
@@ -220,6 +220,9 @@ public:
// get GPS check status
virtual void get_gps_check_status(uint16_t *val) = 0;
// return the amount the local vertical position changed in the last height reset and the time of the reset
virtual void get_vert_pos_reset(float *delta, uint64_t *time_us) = 0;
protected:
parameters _params; // filter parameters