fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: don't reset quaternion states unnecessarily

Browse Source 
When performing the initial in-flight mag yaw reset for RW vehicle, do not reset the quaternion states and corresponding variances unless there has been a change in yaw angle large enough to cause problems with navigation.
This is because the state estimates after a reset are more vulnerable to transient sensor errors, so a reset should be avoided if possible.
This commit is contained in:
Paul Riseborough 2017-11-09 11:32:57 +11:00
parent 9e47b6e1b6
commit 8f27d3fc54
1 changed files with 68 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

126
EKF/ekf_helper.cpp
View File

@ -492,6 +492,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
{
// save a copy of the quaternion state for later use in calculating the amount of reset change
Quatf quat_before_reset = _state.quat_nominal;
Quatf quat_after_reset = _state.quat_nominal;
// calculate the variance for the rotation estimate expressed as a rotation vector
// this will be used later to reset the quaternion state covariances
@ -532,26 +533,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
// calculate initial quaternion states for the ekf
// we don't change the output attitude to avoid jumps
_state.quat_nominal = Quatf(euler321);
// calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal;
// 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);
output_states.quat_nominal = _state_reset_status.quat_change * output_states.quat_nominal;
_output_buffer.push_to_index(i, output_states);
}
// apply the change in attitude quaternion to our newest quaternion estimate
// which was already taken out from the output buffer
_output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal;
// capture the reset event
_state_reset_status.quat_counter++;
quat_after_reset = Quatf(euler321);
} else {
// use a 312 sequence
@ -618,32 +600,12 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
// calculate initial quaternion states for the ekf
// we don't change the output attitude to avoid jumps
_state.quat_nominal = Quatf(R_to_earth);
quat_after_reset = Quatf(R_to_earth);
}
// update transformation matrix from body to world frame using the current estimate
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
// reset the rotation from the EV to EKF frame of reference if it is being used
if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !(_params.fusion_mode & MASK_USE_EVYAW)) {
resetExtVisRotMat();
}
// update the yaw angle variance using the variance of the measurement
if (_params.fusion_mode & MASK_USE_EVYAW) {
// using error estimate from external vision data
angle_err_var_vec(2) = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) {
// using magnetic heading tuning parameter
angle_err_var_vec(2) = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
}
// reset the quaternion covariances using the rotation vector variances
initialiseQuatCovariances(angle_err_var_vec);
// calculate initial earth magnetic field states
_state.mag_I = _R_to_earth * mag_init;
// set the earth magnetic field states using the updated rotation
Dcmf _R_to_earth_after = quat_to_invrotmat(quat_after_reset);
_state.mag_I = _R_to_earth_after * mag_init;
// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
zeroRows(P, 16, 21);
@ -653,28 +615,76 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
P[index][index] = sq(_params.mag_noise);
}
// record the start time for the magnetic field alignment
// record the time for the magnetic field alignment event
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
// calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal;
Quatf q_error = quat_before_reset.inversed() * quat_after_reset;
q_error.normalize();
// add the reset amount to the output observer buffered data
outputSample output_states = {};
unsigned output_length = _output_buffer.get_length();
// convert the quaternion delta to a delta angle
Vector3f delta_ang_error;
float scalar;
for (unsigned i = 0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal = _state_reset_status.quat_change * output_states.quat_nominal;
_output_buffer.push_to_index(i, output_states);
if (q_error(0) >= 0.0f) {
scalar = -2.0f;
} else {
scalar = 2.0f;
}
// apply the change in attitude quaternion to our newest quaternion estimate
// which was already taken out from the output buffer
_output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal;
delta_ang_error(0) = scalar * q_error(1);
delta_ang_error(1) = scalar * q_error(2);
delta_ang_error(2) = scalar * q_error(3);
// capture the reset event
_state_reset_status.quat_counter++;
// update the quaternion state estimates and corresponding covariances only if the change in angle has been large
if (delta_ang_error.norm() > math::radians(15.0f)) {
// update quaternion states
_state.quat_nominal = quat_after_reset;
// record the state change
_state_reset_status.quat_change = q_error;
// update transformation matrix from body to world frame using the current estimate
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
// reset the rotation from the EV to EKF frame of reference if it is being used
if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !(_params.fusion_mode & MASK_USE_EVYAW)) {
resetExtVisRotMat();
}
// update the yaw angle variance using the variance of the measurement
if (_params.fusion_mode & MASK_USE_EVYAW) {
// using error estimate from external vision data
angle_err_var_vec(2) = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) {
// using magnetic heading tuning parameter
angle_err_var_vec(2) = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
}
// reset the quaternion covariances using the rotation vector variances
initialiseQuatCovariances(angle_err_var_vec);
// 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);
output_states.quat_nominal = _state_reset_status.quat_change * output_states.quat_nominal;
_output_buffer.push_to_index(i, output_states);
}
// apply the change in attitude quaternion to our newest quaternion estimate
// which was already taken out from the output buffer
_output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal;
// capture the reset event
_state_reset_status.quat_counter++;
}
return true;
}