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

Merge pull request #323 from PX4/pr-ekfQuatMultOrder

Browse Source 
EKF: use hamiltonian convention for quaternion multiplication order
This commit is contained in:
Paul Riseborough
2017-09-21 07:49:35 +10:00
committed by GitHub
parent 4411c7fd1c dd5b8525c3
commit 160e4d69c1
4 changed files with 18 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+3 -3
View File
@@ -187,20 +187,20 @@ void Ekf::controlExternalVisionFusion()
_state.quat_nominal = Quatf(euler_init);
// calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
_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 = _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 = _state_reset_status.quat_change * _output_new.quat_nominal;
// capture the reset event
_state_reset_status.quat_counter++;