diff --git a/EKF/control.cpp b/EKF/control.cpp index d0f83459c5..61a8d076fe 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -226,12 +226,11 @@ void Ekf::controlExternalVisionFusion() increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f))); // 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 - // Note q1 *= q2 is equivalent to q1 = q2 * q1 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 diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 3429c24465..072b6f233a 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -378,8 +378,8 @@ void Ekf::resetHeight() // align output filter states to match EKF states at the fusion time horizon void Ekf::alignOutputFilter() { - // calculate the quaternion delta between the output and EKF quaternions at the EKF fusion time horizon - Quatf q_delta = _output_sample_delayed.quat_nominal.inversed() * _state.quat_nominal; + // calculate the quaternion rotation delta from the EKF to output observer states at the EKF fusion time horizon + Quatf q_delta = _state.quat_nominal * _output_sample_delayed.quat_nominal.inversed(); q_delta.normalize(); // calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon @@ -388,16 +388,16 @@ void Ekf::alignOutputFilter() // loop through the output filter state history and add the deltas 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; _output_buffer[i].quat_nominal.normalize(); _output_buffer[i].vel += vel_delta; _output_buffer[i].pos += pos_delta; } - _output_new.quat_nominal *= q_delta; + _output_new.quat_nominal = q_delta * _output_new.quat_nominal; _output_new.quat_nominal.normalize(); - _output_sample_delayed.quat_nominal *= q_delta; + _output_sample_delayed.quat_nominal = q_delta * _output_sample_delayed.quat_nominal; _output_sample_delayed.quat_nominal.normalize(); } diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index dea3aad212..007a40f08d 100644 --- a/EKF/gps_yaw_fusion.cpp +++ b/EKF/gps_yaw_fusion.cpp @@ -367,7 +367,7 @@ bool Ekf::resetGpsAntYaw() } // calculate the amount that the quaternion has changed by - Quatf q_error = quat_before_reset.inversed() * _state.quat_nominal; + Quatf q_error = _state.quat_nominal * quat_before_reset.inversed(); q_error.normalize(); // convert the quaternion delta to a delta angle @@ -407,8 +407,7 @@ bool Ekf::resetGpsAntYaw() // add the reset amount to the output observer buffered data 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 * _output_buffer[i].quat_nominal; } // apply the change in attitude quaternion to our newest quaternion estimate