EKF: Remove use of of quaternion self product operator and fix delta …

…rotation sign
PX4 · Jun 17, 2019 · a036cf8 · a036cf8
1 parent 0e946f2
commit a036cf8
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 11 deletions.
diff --git 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
@@ -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
@@ -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