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

EKF: rework height reset

Browse Source 
calculate and save the amount that the vertical states have changed and the time of the change
apply the change delta to the output observer states and buffer
This commit is contained in:
Paul Riseborough
2016-04-22 08:43:56 +10:00
parent e3365525c2
commit 469a7d1711
1 changed files with 48 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
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);
}
}