dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Improve output observer position and velocity tracking 

Replace the delayed time feedback mechanism used by the translational states with a direct feedback method.
Time constants for velocity and position convergence can be separately adjusted with tunable parameters
The method is more computationally more expensive because it requires modification of the output buffer history but is acceptable because it only requires 6 FLOP per buffer index for a total of 30*6 = 180 FLOP
The method was not applied to the attitude states because the quaternion operations required at each buffer index would have been computationally prohibitive.
This commit is contained in:
Paul Riseborough 2016-05-14 10:34:01 +09:30
parent 627d08ecc9
commit c7e225124c
3 changed files with 67 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
EKF/common.h
View File

@ -229,6 +229,10 @@ struct parameters {
Vector3f rng_pos_body; // xyz position of range sensor in body frame (m)
Vector3f flow_pos_body; // xyz position of range sensor focal point in body frame (m)
// output complementary filter tuning
float vel_Tau; // velocity state correction time constant (1/sec)
float pos_Tau; // postion state correction time constant (1/sec)
// Initialize parameter values. Initialization must be accomplished in the constructor to allow C99 compiler compatibility.
parameters()
{
@ -307,6 +311,10 @@ struct parameters {
gps_pos_body = {};
rng_pos_body = {};
flow_pos_body = {};
// output complementary filter tuning time constants
vel_Tau = 0.5f;
pos_Tau = 0.25f;
}
};

106
EKF/ekf.cpp
View File

@ -120,8 +120,6 @@ Ekf::Ekf():
memset(_mag_innov_var, 0, sizeof(_mag_innov_var));
memset(_flow_innov_var, 0, sizeof(_flow_innov_var));
_delta_angle_corr.setZero();
_delta_vel_corr.setZero();
_vel_corr.setZero();
_last_known_posNE.setZero();
_imu_down_sampled = {};
_q_down_sampled.setZero();
@ -154,9 +152,6 @@ bool Ekf::init(uint64_t timestamp)
_output_new.quat_nominal = matrix::Quaternion<float>();
_delta_angle_corr.setZero();
_delta_vel_corr.setZero();
_vel_corr.setZero();
_imu_down_sampled.delta_ang.setZero();
_imu_down_sampled.delta_vel.setZero();
_imu_down_sampled.delta_ang_dt = 0.0f;
@ -657,8 +652,7 @@ void Ekf::calculateOutputStates()
_R_to_earth_now = quat_to_invrotmat(_output_new.quat_nominal);
// rotate the delta velocity to earth frame
// apply a delta velocity correction required to track the velocity states at the EKF fusion time horizon
Vector3f delta_vel_NED = _R_to_earth_now * delta_vel + _delta_vel_corr;
Vector3f delta_vel_NED = _R_to_earth_now * delta_vel;
// corrrect for measured accceleration due to gravity
delta_vel_NED(2) += _gravity_mss * imu_new.delta_vel_dt;
@ -670,50 +664,68 @@ void Ekf::calculateOutputStates()
_output_new.vel += delta_vel_NED;
// use trapezoidal integration to calculate the INS position states
// apply a velocity correction required to track the position states at the EKF fusion time horizon
_output_new.pos += (_output_new.vel + vel_last) * (imu_new.delta_vel_dt * 0.5f) + _vel_corr * imu_new.delta_vel_dt;
_output_new.pos += (_output_new.vel + vel_last) * (imu_new.delta_vel_dt * 0.5f);
// store INS states in a ring buffer that with the same length and time coordinates as the IMU data buffer
if (_imu_updated) {
_output_buffer.push(_output_new);
_imu_updated = false;
// get the oldest INS state data from the ring buffer
// this data will be at the EKF fusion time horizon
_output_sample_delayed = _output_buffer.get_oldest();
// calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon
Quaternion quat_inv = _state.quat_nominal.inversed();
Quaternion q_error = _output_sample_delayed.quat_nominal * quat_inv;
q_error.normalize();
// convert the quaternion delta to a delta angle
Vector3f delta_ang_error;
float scalar;
if (q_error(0) >= 0.0f) {
scalar = -2.0f;
} else {
scalar = 2.0f;
}
delta_ang_error(0) = scalar * q_error(1);
delta_ang_error(1) = scalar * q_error(2);
delta_ang_error(2) = scalar * q_error(3);
// calculate a gain that provides tight tracking of the estimator attitude states and
// adjust for changes in time delay to mantain consistent damping ratio of ~0.7
float time_delay = 1e-6f * (float)(_imu_sample_new.time_us - _imu_sample_delayed.time_us);
time_delay = fmaxf(time_delay, _dt_imu_avg);
float att_gain = 0.5f * _dt_imu_avg / time_delay;
// calculate a corrrection to the delta angle
// that will cause the INS to track the EKF quaternions
_delta_angle_corr = delta_ang_error * att_gain;
// calculate gains that will be used to make the INS states converge on the EKF states
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);
// 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;
// loop through the output filter state history and apply the corrections to the translational states
// 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 trackin gtime constants
// to be used
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);
output_states.vel += vel_delta;
output_states.pos += pos_delta;
_output_buffer.push_to_index(i,output_states);
}
// update output state to corrected values
_output_new = _output_buffer.get_newest();
}
// get the oldest INS state data from the ring buffer
// this data will be at the EKF fusion time horizon
_output_sample_delayed = _output_buffer.get_oldest();
// calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon
Quaternion quat_inv = _state.quat_nominal.inversed();
Quaternion q_error = _output_sample_delayed.quat_nominal * quat_inv;
q_error.normalize();
// convert the quaternion delta to a delta angle
Vector3f delta_ang_error;
float scalar;
if (q_error(0) >= 0.0f) {
scalar = -2.0f;
} else {
scalar = 2.0f;
}
delta_ang_error(0) = scalar * q_error(1);
delta_ang_error(1) = scalar * q_error(2);
delta_ang_error(2) = scalar * q_error(3);
// calculate gains that provides tight tracking of the estimator states and
// adjust for changes in time delay to mantain consistent overshoot
float omega = 1e6f / (_imu_sample_new.time_us - _imu_sample_delayed.time_us);
// calculate a corrrection to the delta angle
// that will cause the INS to track the EKF quaternions
_delta_angle_corr = delta_ang_error * imu_new.delta_ang_dt * omega * 0.5f;
// calculate a correction to the delta velocity
// that will cause the INS to track the EKF velocity
_delta_vel_corr = (_state.vel - _output_sample_delayed.vel) * imu_new.delta_vel_dt * omega * 0.5f;
// calculate a correction to the INS velocity
// that will cause the INS to track the EKF position
_vel_corr = (_state.pos - _output_sample_delayed.pos) * omega * 0.6f;
}

2
EKF/ekf.h
View File

@ -192,8 +192,6 @@ private:
// complementary filter states
Vector3f _delta_angle_corr; // delta angle correction vector
Vector3f _delta_vel_corr; // delta velocity correction vector
Vector3f _vel_corr; // velocity correction vector
imuSample _imu_down_sampled; // down sampled imu data (sensor rate -> filter update rate)
Quaternion _q_down_sampled; // down sampled quaternion (tracking delta angles between ekf update steps)