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

EKF: Clarify use of *= operator for quaternions

This commit is contained in:
Paul Riseborough 2017-11-16 11:52:19 +11:00
parent ed9a394029
commit 7c8fcf7628
2 changed files with 4 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
EKF/control.cpp
View File

@ -199,6 +199,7 @@ void Ekf::controlExternalVisionFusion()
_state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal; _state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal;
// 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 = q2 * q1
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;
} }

3
EKF/ekf_helper.cpp
View File

@ -318,6 +318,7 @@ void Ekf::alignOutputFilter()
const Vector3f pos_delta = _state.pos - _output_sample_delayed.pos; const Vector3f pos_delta = _state.pos - _output_sample_delayed.pos;
// loop through the output filter state history and add the deltas // loop through the output filter state history and add the deltas
// Note q1 *= q2 is equivalent to q1 = q2 * q1
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 *= q_delta; _output_buffer[i].quat_nominal *= q_delta;
_output_buffer[i].quat_nominal.normalize(); _output_buffer[i].quat_nominal.normalize();
@ -429,6 +430,7 @@ bool Ekf::realignYawGPS()
_state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal; _state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal;
// 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 = q2 * q1
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;
} }
@ -651,6 +653,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
// 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 = q2 * q1
_output_buffer[i].quat_nominal *= _state_reset_status.quat_change; _output_buffer[i].quat_nominal *= _state_reset_status.quat_change;
} }