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

ekf run output predictor immediately with new IMU data (#471)

Browse Source 
* EKF collect_imu take const imu sample and populate buffer

* EKF calculateOutputStates cleanup

* EKF add calculate_quaternion output predictor method

* EKF: update documentation

* EKF: remove unnecessary getter function

* EKF calculateOutputStates only apply dt correction to bias

* EKF pytest assert attitude validity, not update() return

* EKF: correct documentation

* EKF: Do not make attitude validity dependent on yaw alignment status

Yaw alignment could fail in flight due to temporary loss of data and yet the quaternions would still usable for stabilisation even though the absolute earth yaw angle wrt true north was uncertain.
This commit is contained in:
Daniel Agar
2018-07-05 07:59:35 +10:00
committed by Paul Riseborough
parent 114ae4116a
commit cebdc3d829
6 changed files with 104 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+83 -66
View File
@@ -99,6 +99,8 @@ bool Ekf::init(uint64_t timestamp)
bool Ekf::update()
{
bool updated = false;
if (!_filter_initialised) {
_filter_initialised = initialiseFilter();
@@ -120,18 +122,14 @@ bool Ekf::update()
// run a separate filter for terrain estimation
runTerrainEstimator();
updated = true;
}
// the output observer always runs
// Use full rate IMU data at the current time horizon
calculateOutputStates();
// check for NaN or inf on attitude states
if (!ISFINITE(_state.quat_nominal(0)) || !ISFINITE(_output_new.quat_nominal(0))) {
return false;
}
// We don't have valid data to output until tilt and yaw alignment is complete
return _control_status.flags.tilt_align && _control_status.flags.yaw_align;
return updated;
}
bool Ekf::initialiseFilter()
@@ -365,7 +363,7 @@ void Ekf::predictState()
_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input;
}
bool Ekf::collect_imu(imuSample &imu)
bool Ekf::collect_imu(const imuSample &imu)
{
// accumulate and downsample IMU data across a period FILTER_UPDATE_PERIOD_MS long
@@ -389,7 +387,7 @@ bool Ekf::collect_imu(imuSample &imu)
// accumulate the most recent delta velocity data at the updated rotation frame
// assume effective sample time is halfway between the previous and current rotation frame
_imu_down_sampled.delta_vel += (_imu_sample_new.delta_vel + delta_R * _imu_sample_new.delta_vel) * 0.5f;
_imu_down_sampled.delta_vel += (imu.delta_vel + delta_R * imu.delta_vel) * 0.5f;
// if the target time delta between filter prediction steps has been exceeded
// write the accumulated IMU data to the ring buffer
@@ -402,11 +400,23 @@ bool Ekf::collect_imu(imuSample &imu)
_imu_collection_time_adj += 0.01f * (_imu_down_sampled.delta_ang_dt - target_dt);
_imu_collection_time_adj = math::constrain(_imu_collection_time_adj, -0.5f * target_dt, 0.5f * target_dt);
imu.delta_ang = _q_down_sampled.to_axis_angle();
imu.delta_vel = _imu_down_sampled.delta_vel;
imu.delta_ang_dt = _imu_down_sampled.delta_ang_dt;
imu.delta_vel_dt = _imu_down_sampled.delta_vel_dt;
imuSample imu_sample_new;
imu_sample_new.delta_ang = _q_down_sampled.to_axis_angle();
imu_sample_new.delta_vel = _imu_down_sampled.delta_vel;
imu_sample_new.delta_ang_dt = _imu_down_sampled.delta_ang_dt;
imu_sample_new.delta_vel_dt = _imu_down_sampled.delta_vel_dt;
imu_sample_new.time_us = imu.time_us;
_imu_buffer.push(imu_sample_new);
// get the oldest data from the buffer
_imu_sample_delayed = _imu_buffer.get_oldest();
// calculate the minimum interval between observations required to guarantee no loss of data
// this will occur if data is overwritten before its time stamp falls behind the fusion time horizon
_min_obs_interval_us = (imu_sample_new.time_us - _imu_sample_delayed.time_us) / (_obs_buffer_length - 1);
// reset
_imu_down_sampled.delta_ang.setZero();
_imu_down_sampled.delta_vel.setZero();
_imu_down_sampled.delta_ang_dt = 0.0f;
@@ -414,10 +424,13 @@ bool Ekf::collect_imu(imuSample &imu)
_q_down_sampled(0) = 1.0f;
_q_down_sampled(1) = _q_down_sampled(2) = _q_down_sampled(3) = 0.0f;
return true;
_imu_updated = true;
} else {
_imu_updated = false;
}
return false;
return _imu_updated;
}
/*
@@ -433,37 +446,30 @@ bool Ekf::collect_imu(imuSample &imu)
void Ekf::calculateOutputStates()
{
// Use full rate IMU data at the current time horizon
const imuSample &imu_new = _imu_sample_new;
const imuSample &imu = _imu_sample_new;
// correct delta angles for bias offsets
Vector3f delta_angle;
float dt_scale_correction = _dt_imu_avg / _dt_ekf_avg;
delta_angle(0) = _imu_sample_new.delta_ang(0) - _state.gyro_bias(0) * dt_scale_correction;
delta_angle(1) = _imu_sample_new.delta_ang(1) - _state.gyro_bias(1) * dt_scale_correction;
delta_angle(2) = _imu_sample_new.delta_ang(2) - _state.gyro_bias(2) * dt_scale_correction;
// calculate a yaw change about the earth frame vertical
float spin_del_ang_D = _R_to_earth_now(2, 0) * delta_angle(0) +
_R_to_earth_now(2, 1) * delta_angle(1) +
_R_to_earth_now(2, 2) * delta_angle(2);
_yaw_delta_ef += spin_del_ang_D;
// Calculate filtered yaw rate to be used by the magnetomer fusion type selection logic
// Note fixed coefficients are used to save operations. The exact time constant is not important.
_yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / _imu_sample_new.delta_ang_dt;
// correct delta velocity for bias offsets
Vector3f delta_vel = _imu_sample_new.delta_vel - _state.accel_bias * dt_scale_correction;
const float dt_scale_correction = _dt_imu_avg / _dt_ekf_avg;
// Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon
delta_angle += _delta_angle_corr;
const Vector3f delta_angle{imu.delta_ang - _state.gyro_bias * dt_scale_correction + _delta_angle_corr};
// calculate a yaw change about the earth frame vertical
const float spin_del_ang_D = _R_to_earth_now(2, 0) * delta_angle(0) +
_R_to_earth_now(2, 1) * delta_angle(1) +
_R_to_earth_now(2, 2) * delta_angle(2);
_yaw_delta_ef += spin_del_ang_D;
// Calculate filtered yaw rate to be used by the magnetometer fusion type selection logic
// Note fixed coefficients are used to save operations. The exact time constant is not important.
_yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu.delta_ang_dt;
// convert the delta angle to an equivalent delta quaternions
Quatf dq;
dq.from_axis_angle(delta_angle);
// rotate the previous INS quaternion by the delta quaternions
_output_new.time_us = imu_new.time_us;
_output_new.time_us = imu.time_us;
_output_new.quat_nominal = _output_new.quat_nominal * dq;
// the quaternions must always be normalised afer modification
@@ -472,19 +478,22 @@ void Ekf::calculateOutputStates()
// calculate the rotation matrix from body to earth frame
_R_to_earth_now = quat_to_invrotmat(_output_new.quat_nominal);
// rotate the delta velocity to earth frame
Vector3f delta_vel_NED = _R_to_earth_now * delta_vel;
// correct delta velocity for bias offsets
const Vector3f delta_vel{imu.delta_vel - _state.accel_bias * dt_scale_correction};
// corrrect for measured accceleration due to gravity
delta_vel_NED(2) += CONSTANTS_ONE_G * imu_new.delta_vel_dt;
// rotate the delta velocity to earth frame
Vector3f delta_vel_NED{_R_to_earth_now * delta_vel};
// correct for measured acceleration due to gravity
delta_vel_NED(2) += CONSTANTS_ONE_G * imu.delta_vel_dt;
// calculate the earth frame velocity derivatives
if (imu_new.delta_vel_dt > 1e-4f) {
_vel_deriv_ned = delta_vel_NED * (1.0f / imu_new.delta_vel_dt);
if (imu.delta_vel_dt > 1e-4f) {
_vel_deriv_ned = delta_vel_NED * (1.0f / imu.delta_vel_dt);
}
// save the previous velocity so we can use trapezoidal integration
Vector3f vel_last = _output_new.vel;
const Vector3f vel_last{_output_new.vel};
// increment the INS velocity states by the measurement plus corrections
// do the same for vertical state used by alternative correction algorithm
@@ -493,31 +502,29 @@ void Ekf::calculateOutputStates()
// use trapezoidal integration to calculate the INS position states
// do the same for vertical state used by alternative correction algorithm
Vector3f delta_pos_NED = (_output_new.vel + vel_last) * (imu_new.delta_vel_dt * 0.5f);
const Vector3f delta_pos_NED = (_output_new.vel + vel_last) * (imu.delta_vel_dt * 0.5f);
_output_new.pos += delta_pos_NED;
_output_vert_new.vel_d_integ += delta_pos_NED(2);
// accumulate the time for each update
_output_vert_new.dt += imu_new.delta_vel_dt;
_output_vert_new.dt += imu.delta_vel_dt;
// correct velocity for IMU offset
if (_imu_sample_new.delta_ang_dt > 1e-4f) {
if (imu.delta_ang_dt > 1e-4f) {
// calculate the average angular rate across the last IMU update
Vector3f ang_rate = _imu_sample_new.delta_ang * (1.0f / _imu_sample_new.delta_ang_dt);
const Vector3f ang_rate = imu.delta_ang * (1.0f / imu.delta_ang_dt);
// calculate the velocity of the IMU relative to the body origin
Vector3f vel_imu_rel_body = cross_product(ang_rate, _params.imu_pos_body);
const Vector3f vel_imu_rel_body = cross_product(ang_rate, _params.imu_pos_body);
// rotate the relative velocity into earth frame
_vel_imu_rel_body_ned = _R_to_earth_now * vel_imu_rel_body;
}
// store the INS states in a ring buffer with the same length and time coordinates as the IMU data buffer
if (_imu_updated) {
_output_buffer.push(_output_new);
_output_vert_buffer.push(_output_vert_new);
_imu_updated = false;
// get the oldest INS state data from the ring buffer
// this data will be at the EKF fusion time horizon
@@ -530,7 +537,6 @@ void Ekf::calculateOutputStates()
q_error.normalize();
// convert the quaternion delta to a delta angle
Vector3f delta_ang_error;
float scalar;
if (q_error(0) >= 0.0f) {
@@ -540,23 +546,20 @@ void Ekf::calculateOutputStates()
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);
const Vector3f delta_ang_error{scalar * q_error(1), scalar * q_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 maintain 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;
const float time_delay = fmaxf((imu.time_us - _imu_sample_delayed.time_us) * 1e-6f, _dt_imu_avg);
const 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 velocity and position tracking errors
Vector3f vel_err = (_state.vel - _output_sample_delayed.vel);
Vector3f pos_err = (_state.pos - _output_sample_delayed.pos);
const Vector3f vel_err{_state.vel - _output_sample_delayed.vel};
const Vector3f pos_err{_state.pos - _output_sample_delayed.pos};
// collect magnitude tracking error for diagnostics
_output_tracking_error[0] = delta_ang_error.norm();
@@ -571,8 +574,8 @@ void Ekf::calculateOutputStates()
*/
// Complementary filter gains
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);
const float vel_gain = _dt_ekf_avg / math::constrain(_params.vel_Tau, _dt_ekf_avg, 10.0f);
const float pos_gain = _dt_ekf_avg / math::constrain(_params.pos_Tau, _dt_ekf_avg, 10.0f);
{
/*
* Calculate a correction to be applied to vel_d that casues vel_d_integ to track the EKF
@@ -584,12 +587,12 @@ void Ekf::calculateOutputStates()
*/
// calculate down velocity and position tracking errors
float vel_d_err = (_state.vel(2) - _output_vert_delayed.vel_d);
float pos_d_err = (_state.pos(2) - _output_vert_delayed.vel_d_integ);
const float vel_d_err = (_state.vel(2) - _output_vert_delayed.vel_d);
const float pos_d_err = (_state.pos(2) - _output_vert_delayed.vel_d_integ);
// calculate a velocity correction that will be applied to the output state history
// using a PD feedback tuned to a 5% overshoot
float vel_d_correction = pos_d_err * pos_gain + vel_d_err * pos_gain * 1.1f;
const float vel_d_correction = pos_d_err * pos_gain + vel_d_err * pos_gain * 1.1f;
/*
* Calculate corrections to be applied to vel and pos output state history.
@@ -638,11 +641,11 @@ void Ekf::calculateOutputStates()
// calculate a velocity correction that will be applied to the output state history
_vel_err_integ += vel_err;
Vector3f vel_correction = vel_err * vel_gain + _vel_err_integ * sq(vel_gain) * 0.1f;
const Vector3f vel_correction = vel_err * vel_gain + _vel_err_integ * sq(vel_gain) * 0.1f;
// calculate a position correction that will be applied to the output state history
_pos_err_integ += pos_err;
Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f;
const Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f;
// loop through the output filter state history and apply the corrections to the velocity and position states
for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
@@ -658,3 +661,17 @@ void Ekf::calculateOutputStates()
}
}
}
/*
* Predict the previous quaternion output state forward using the latest IMU delta angle data.
*/
Quatf Ekf::calculate_quaternion() const
{
// Correct delta angle data for bias errors using bias state estimates from the EKF and also apply
// corrections required to track the EKF quaternion states
const Vector3f delta_angle{_imu_sample_new.delta_ang - _state.gyro_bias * (_dt_imu_avg / _dt_ekf_avg) + _delta_angle_corr};
// increment the quaternions using the corrected delta angle vector
// the quaternions must always be normalised after modification
return Quatf{_output_new.quat_nominal * AxisAnglef{delta_angle}}.unit();
}