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

EKF: Add output predictor processing option

Browse Source 
Setting the velocity tracking tine constant to a negative number causes the output predictor to use a different method of correcting the velocity which provides a velocity output that is kinematically consistent with the position output.

This may improve height controller performance under some circumstances
This commit is contained in:
Paul Riseborough
2016-05-23 07:47:10 +10:00
parent 30917430e2
commit 90d65071c1
1 changed files with 26 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+26 -11
View File
@@ -742,25 +742,40 @@ void Ekf::calculateOutputStates()
// that will cause the INS to track the EKF quaternions // that will cause the INS to track the EKF quaternions
_delta_angle_corr = delta_ang_error * att_gain; _delta_angle_corr = delta_ang_error * att_gain;
// calculate gains that will be used to make the INS states converge on the EKF states // calculate a position correction that will be applied to the output state history
float vel_gain = _dt_ekf_avg / math::constrain(_params.vel_Tau, _dt_ekf_avg, 10.0f);
float pos_gain = _dt_ekf_avg / math::constrain(_params.pos_Tau, _dt_ekf_avg, 10.0f); float pos_gain = _dt_ekf_avg / math::constrain(_params.pos_Tau, _dt_ekf_avg, 10.0f);
// calculate velocity and position corrections at the EKF fusion time horizon
Vector3f vel_delta = (_state.vel - _output_sample_delayed.vel) * vel_gain;
Vector3f pos_delta = (_state.pos - _output_sample_delayed.pos) * pos_gain; Vector3f pos_delta = (_state.pos - _output_sample_delayed.pos) * pos_gain;
// loop through the output filter state history and apply the corrections to the translational states // calculate a velocity correction that will be applied to the output state history
Vector3f vel_delta;
if (_params.pos_Tau <= 0.0f) {
// this method will cause the velocity to be kinematically consistent with
// the position corretions rather than tracking the EKF states
vel_delta = pos_delta * (1.0f / time_delay);
} else {
// this method makes the velocity track the EKF states with the specified time constant
float vel_gain = _dt_ekf_avg / math::constrain(_params.vel_Tau, _dt_ekf_avg, 10.0f);
vel_delta = (_state.vel - _output_sample_delayed.vel) * vel_gain;
}
// loop through the output filter state history and apply the corrections to the velocity and position states
// this method is too expensive to use for the attitude states due to the quaternion operations required // this method is too expensive to use for the attitude states due to the quaternion operations required
// but does not introudce a time dela in the 'correction loop' and allows smaller tracking time constants // but does not introduce a time delay in the 'correction loop' and allows smaller tracking time constants
// to be used // to be used
outputSample output_states; outputSample output_states;
unsigned output_length = _output_buffer.get_length(); unsigned max_index = _output_buffer.get_length() - 1;
for (unsigned i=0; i < output_length; i++) { for (unsigned index=0; index <= max_index; index++) {
output_states = _output_buffer.get_from_index(i); output_states = _output_buffer.get_from_index(index);
// a constant velocity correction is applied
output_states.vel += vel_delta; output_states.vel += vel_delta;
// a constant position correction is applied
output_states.pos += pos_delta; output_states.pos += pos_delta;
_output_buffer.push_to_index(i,output_states);
// push the updated data to the buffer
_output_buffer.push_to_index(index,output_states);
} }
// update output state to corrected values // update output state to corrected values