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

EKF: Remove use of quaternion self product operator and fix delta rotation sign error

Browse Source
This commit is contained in:
Paul Riseborough
2019-06-14 09:47:44 +10:00
committed by Paul Riseborough
parent ad7f7af03b
commit 0e946f25fd
1 changed files with 6 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+6 -8
View File
@@ -506,17 +506,16 @@ bool Ekf::realignYawGPS()
_flt_mag_align_start_time = _imu_sample_delayed.time_us; _flt_mag_align_start_time = _imu_sample_delayed.time_us;
// calculate the amount that the quaternion has changed by // calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal; _state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
// add the reset amount to the output observer buffered data // add the reset amount to the output observer buffered data
// Note q1 *= q2 is equivalent to q1 = q1 * q2
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
_output_buffer[i].quat_nominal *= _state_reset_status.quat_change; _output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
} }
// apply the change in attitude quaternion to our newest quaternion estimate // apply the change in attitude quaternion to our newest quaternion estimate
// which was already taken out from the output buffer // which was already taken out from the output buffer
_output_new.quat_nominal = _output_new.quat_nominal * _state_reset_status.quat_change; _output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal;
// capture the reset event // capture the reset event
_state_reset_status.quat_counter++; _state_reset_status.quat_counter++;
@@ -718,7 +717,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update
_flt_mag_align_start_time = _imu_sample_delayed.time_us; _flt_mag_align_start_time = _imu_sample_delayed.time_us;
// calculate the amount that the quaternion has changed by // calculate the amount that the quaternion has changed by
Quatf q_error = quat_before_reset.inversed() * quat_after_reset; Quatf q_error = quat_after_reset * quat_before_reset.inversed();
q_error.normalize(); q_error.normalize();
// update quaternion states // update quaternion states
@@ -750,13 +749,12 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update
if (update_buffer) { if (update_buffer) {
// add the reset amount to the output observer buffered data // add the reset amount to the output observer buffered data
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
// Note q1 *= q2 is equivalent to q1 = q1 * q2 _output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
_output_buffer[i].quat_nominal *= _state_reset_status.quat_change;
} }
// apply the change in attitude quaternion to our newest quaternion estimate // apply the change in attitude quaternion to our newest quaternion estimate
// which was already taken out from the output buffer // which was already taken out from the output buffer
_output_new.quat_nominal = _output_new.quat_nominal * _state_reset_status.quat_change; _output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal;
} }
// capture the reset event // capture the reset event