From b57b5e869d5a9159676d4273d89e1c2406183ac7 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 30 Aug 2017 13:43:56 +0200 Subject: [PATCH 1/2] matrix: Use hamiltonian convention for quaternion product --- matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/matrix b/matrix index cf924956d7..e595ebb9a7 160000 --- a/matrix +++ b/matrix @@ -1 +1 @@ -Subproject commit cf924956d7d62ce18bfc4f8441e9177ddb69c0dc +Subproject commit e595ebb9a704c24aeae990dac768d26949fcaee0 From dd5b8525c391bc4e21907f0074754b8f53c7d80a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 30 Aug 2017 15:08:02 +0200 Subject: [PATCH 2/2] EKF: Use hamiltonian convention for quaternion product order --- EKF/control.cpp | 6 +++--- EKF/ekf.cpp | 6 +++--- EKF/ekf_helper.cpp | 22 +++++++++++----------- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 75d6dd92fb..edef5000ad 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -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++; diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index a93cf795d7..e20b5d9116 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -329,7 +329,7 @@ void Ekf::predictState() dq.from_axis_angle(corrected_delta_ang); // rotate the previous quaternion by the delta quaternion using a quaternion multiplication - _state.quat_nominal = dq * _state.quat_nominal; + _state.quat_nominal = _state.quat_nominal * dq; // quaternions must be normalised whenever they are modified _state.quat_nominal.normalize(); @@ -471,7 +471,7 @@ void Ekf::calculateOutputStates() // rotate the previous INS quaternion by the delta quaternions _output_new.time_us = imu_new.time_us; - _output_new.quat_nominal = dq * _output_new.quat_nominal; + _output_new.quat_nominal = _output_new.quat_nominal * dq; // the quaternions must always be normalised afer modification _output_new.quat_nominal.normalize(); @@ -524,7 +524,7 @@ void Ekf::calculateOutputStates() // calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon Quatf quat_inv = _state.quat_nominal.inversed(); - Quatf q_error = _output_sample_delayed.quat_nominal * quat_inv; + Quatf q_error = quat_inv * _output_sample_delayed.quat_nominal; q_error.normalize(); // convert the quaternion delta to a delta angle diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 34d3142de1..835ed6a44b 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -319,7 +319,7 @@ void Ekf::alignOutputFilter() { // calculate the quaternion delta between the output and EKF quaternions at the EKF fusion time horizon Quatf quat_inv = _state.quat_nominal.inversed(); - Quatf q_delta = _output_sample_delayed.quat_nominal * quat_inv; + Quatf q_delta = quat_inv * _output_sample_delayed.quat_nominal; q_delta.normalize(); // calculate the velocity and posiiton deltas between the output and EKF at the EKF fusion time horizon @@ -332,7 +332,7 @@ void Ekf::alignOutputFilter() for (unsigned i = 0; i < output_length; i++) { output_states = _output_buffer.get_from_index(i); - output_states.quat_nominal *= q_delta; + output_states.quat_nominal = q_delta * output_states.quat_nominal; output_states.quat_nominal.normalize(); output_states.vel += vel_delta; output_states.pos += pos_delta; @@ -439,20 +439,20 @@ bool Ekf::realignYawGPS() } // 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++; @@ -522,20 +522,20 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) _state.quat_nominal = Quatf(euler321); // 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++; @@ -636,7 +636,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_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 = {}; @@ -644,13 +644,13 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) 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++;