EKF: fix the rotation direction of the buffered attitude solutions

in alignOutputFilter() Signed-off-by: CarlOlsson <carlolsson.co@gmail.com>
6 years ago · 78122b20a6
1 changed files with 1 additions and 1 deletions
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@ -379,7 +379,7 @@ void Ekf::resetHeight()
				@@ -379,7 +379,7 @@ void Ekf::resetHeight()
 void Ekf::alignOutputFilter()
 {
 	// calculate the quaternion delta between the output and EKF quaternions at the EKF fusion time horizon
-	Quatf q_delta = _state.quat_nominal.inversed() * _output_sample_delayed.quat_nominal;
+	Quatf q_delta = _state.quat_nominal * _output_sample_delayed.quat_nominal.inversed();
 	q_delta.normalize();

 	// calculate the velocity and posiiton deltas between the output and EKF at the EKF fusion time horizon