Skip to content

Commit

Permalink
Merge pull request PX4#102 from priseborough/pr-ekf2HeightReset
Browse files Browse the repository at this point in the history 
EKF: Improve height reset behaviour
  • Loading branch information
@priseborough
priseborough committed Apr 23, 2016
2 parents 1381468 + 4fcbfb5 commit 4263590
Show file tree
Hide file tree
Showing 7 changed files with 233 additions and 41 deletions.
24 changes: 24 additions & 0 deletions EKF/RingBuffer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -160,6 +160,30 @@ class RingBuffer
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;
Expand Down
2 changes: 1 addition & 1 deletion EKF/common.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
139 changes: 120 additions & 19 deletions EKF/control.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}
}
Expand All @@ -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
Expand All @@ -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;
}
}

Expand All @@ -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;
}
}

Expand All @@ -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
Expand Down
39 changes: 23 additions & 16 deletions EKF/ekf.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand All @@ -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();
Expand Down Expand Up @@ -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;
}
}

Expand All @@ -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;
}
}

Expand Down Expand Up @@ -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
Expand All @@ -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;
}
}
Expand Down
18 changes: 14 additions & 4 deletions EKF/ekf.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -126,6 +126,9 @@ class Ekf : public EstimatorInterface
// 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;
Expand Down Expand Up @@ -169,7 +172,7 @@ class Ekf : public EstimatorInterface
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
Expand Down Expand Up @@ -208,12 +211,12 @@ class Ekf : public EstimatorInterface

// 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;

Expand All @@ -231,7 +234,14 @@ class Ekf : public EstimatorInterface
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
Expand Down
Loading

0 comments on commit 4263590

Please sign in to comment.