Test flight has been performed with nonlinear SO(3) attitude estimator.

Here are few observations:
 - When the system initialized, roll angle is initially reversed.
   As filter converged, it becomes normal.
 - I put a negative sign on roll, yaw. It should naturally has right
   sign, but I do not know why for now. Let me investigate again.
 - Gain : I do not know what gain is good for quadrotor flight.
   Let me take a look Ardupilot gain in the later.

Anyway, you can fly with this attitude estimator.

2 files changed, 116 insertions, 114 deletions

diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig
index 02e224302..94d99112e 100755
--- a/nuttx/configs/px4fmu/nsh/defconfig
+++ b/nuttx/configs/px4fmu/nsh/defconfig
@@ -248,7 +248,7 @@ CONFIG_SERIAL_TERMIOS=y
 CONFIG_SERIAL_CONSOLE_REINIT=y
 CONFIG_STANDARD_SERIAL=y
 
-CONFIG_USART1_SERIAL_CONSOLE=y
+CONFIG_USART1_SERIAL_CONSOLE=n
 CONFIG_USART2_SERIAL_CONSOLE=n
 CONFIG_USART3_SERIAL_CONSOLE=n
 CONFIG_UART4_SERIAL_CONSOLE=n
@@ -561,7 +561,7 @@ CONFIG_START_MONTH=1
 CONFIG_START_DAY=1
 CONFIG_GREGORIAN_TIME=n
 CONFIG_JULIAN_TIME=n
-CONFIG_DEV_CONSOLE=y
+CONFIG_DEV_CONSOLE=n
 CONFIG_DEV_LOWCONSOLE=n
 CONFIG_MUTEX_TYPES=n
 CONFIG_PRIORITY_INHERITANCE=y
@@ -925,7 +925,7 @@ CONFIG_USBDEV_TRACE_NRECORDS=512
 #   Size of the serial receive/transmit buffers. Default 256.
 #
 CONFIG_CDCACM=y
-CONFIG_CDCACM_CONSOLE=n
+CONFIG_CDCACM_CONSOLE=y
 #CONFIG_CDCACM_EP0MAXPACKET
 CONFIG_CDCACM_EPINTIN=1
 #CONFIG_CDCACM_EPINTIN_FSSIZE
diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
index ac898eefc..28fcf0369 100755
--- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
+++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
@@ -44,8 +44,9 @@ extern "C" __EXPORT int attitude_estimator_so3_comp_main(int argc, char *argv[])
 static bool thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;		/**< Deamon status flag */
 static int attitude_estimator_so3_comp_task;				/**< Handle of deamon task / thread */
-static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;					// quaternion of sensor frame relative to auxiliary frame
-static float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;	// integral error terms scaled by Ki
+static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;	/** quaternion of sensor frame relative to auxiliary frame */
+static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */
+static float gravity_vector[3] = {0.0f,0.0f,0.0f};	/** estimated gravity vector */
 
 //! Serial packet related
 static int uart;
@@ -151,82 +152,91 @@ float invSqrt(float number) {
 
 void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) {
 	float recipNorm;
-	float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;  
-	float hx, hy, bx, bz;
-	float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
-	float halfex, halfey, halfez;
-
-	// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
+	float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
+	float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
+        	
+	// Auxiliary variables to avoid repeated arithmetic
+        q0q0 = q0 * q0;
+       	q0q1 = q0 * q1;
+       	q0q2 = q0 * q2;
+       	q0q3 = q0 * q3;
+       	q1q1 = q1 * q1;
+       	q1q2 = q1 * q2;
+   	q1q3 = q1 * q3;
+       	q2q2 = q2 * q2;
+       	q2q3 = q2 * q3;
+       	q3q3 = q3 * q3;   
+
+	//! If magnetometer measurement is available, use it.
 	if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
-		//MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az, twoKp, twoKi, dt);
-		return;
+		float hx, hy, bx, bz;
+		float halfwx, halfwy, halfwz;
+	
+		// Normalise magnetometer measurement
+    		recipNorm = invSqrt(mx * mx + my * my + mz * mz);
+    		mx *= recipNorm;
+    		my *= recipNorm;
+    		mz *= recipNorm;
+    
+    		// Reference direction of Earth's magnetic field
+    		hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
+    		hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
+    		bx = sqrt(hx * hx + hy * hy);
+    		bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
+    
+    		// Estimated direction of magnetic field
+    		halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
+    		halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
+    		halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
+    
+    		// Error is sum of cross product between estimated direction and measured direction of field vectors
+    		halfex += (my * halfwz - mz * halfwy);
+    		halfey += (mz * halfwx - mx * halfwz);
+    		halfez += (mx * halfwy - my * halfwx);
 	}
 
 	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
 	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
-
-	// Normalise accelerometer measurement
-	recipNorm = invSqrt(ax * ax + ay * ay + az * az);
-	ax *= recipNorm;
-	ay *= recipNorm;
-	az *= recipNorm;     
-
-	// Normalise magnetometer measurement
-	recipNorm = invSqrt(mx * mx + my * my + mz * mz);
-	mx *= recipNorm;
-	my *= recipNorm;
-	mz *= recipNorm;   
-
-        // Auxiliary variables to avoid repeated arithmetic
-        q0q0 = q0 * q0;
-        q0q1 = q0 * q1;
-        q0q2 = q0 * q2;
-        q0q3 = q0 * q3;
-        q1q1 = q1 * q1;
-        q1q2 = q1 * q2;
-        q1q3 = q1 * q3;
-        q2q2 = q2 * q2;
-        q2q3 = q2 * q3;
-        q3q3 = q3 * q3;   
-
-        // Reference direction of Earth's magnetic field
-        hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
-        hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
-        bx = sqrt(hx * hx + hy * hy);
-        bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
-
-	// Estimated direction of gravity and magnetic field
-	halfvx = q1q3 - q0q2;
-	halfvy = q0q1 + q2q3;
-	halfvz = q0q0 - 0.5f + q3q3;
-        halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
-        halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
-        halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);  
+		float halfvx, halfvy, halfvz;
 	
-	// Error is sum of cross product between estimated direction and measured direction of field vectors
-	halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
-	halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
-	halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
-
-	// Compute and apply integral feedback if enabled
-	if(twoKi > 0.0f) {
-		integralFBx += twoKi * halfex * dt;	// integral error scaled by Ki
-		integralFBy += twoKi * halfey * dt;
-		integralFBz += twoKi * halfez * dt;
-		gx += integralFBx;	// apply integral feedback
-		gy += integralFBy;
-		gz += integralFBz;
-	}
-	else {
-		integralFBx = 0.0f;	// prevent integral windup
-		integralFBy = 0.0f;
-		integralFBz = 0.0f;
+		// Normalise accelerometer measurement
+		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+		ax *= recipNorm;
+		ay *= recipNorm;
+		az *= recipNorm;     
+
+		// Estimated direction of gravity and magnetic field
+		halfvx = q1q3 - q0q2;
+		halfvy = q0q1 + q2q3;
+		halfvz = q0q0 - 0.5f + q3q3;
+	
+		// Error is sum of cross product between estimated direction and measured direction of field vectors
+		halfex += ay * halfvz - az * halfvy;
+		halfey += az * halfvx - ax * halfvz;
+		halfez += ax * halfvy - ay * halfvx;
 	}
 
-	// Apply proportional feedback
-	gx += twoKp * halfex;
-	gy += twoKp * halfey;
-	gz += twoKp * halfez;
+	// Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
+	if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
+		// Compute and apply integral feedback if enabled
+		if(twoKi > 0.0f) {
+			gyro_bias[0] += twoKi * halfex * dt;	// integral error scaled by Ki
+			gyro_bias[1] += twoKi * halfey * dt;
+			gyro_bias[2] += twoKi * halfez * dt;
+			gx += gyro_bias[0];	// apply integral feedback
+			gy += gyro_bias[1];
+			gz += gyro_bias[2];
+		}
+		else {
+			gyro_bias[0] = 0.0f;	// prevent integral windup
+			gyro_bias[1] = 0.0f;
+			gyro_bias[2] = 0.0f;
+		}
+
+		// Apply proportional feedback
+		gx += twoKp * halfex;
+		gy += twoKp * halfey;
+		gz += twoKp * halfez;
 	}
 	
 	// Integrate rate of change of quaternion
@@ -309,11 +319,11 @@ int open_uart(int baud, const char *uart_name, struct termios *uart_config_origi
         case 460800: speed = B460800; break;
         case 921600: speed = B921600; break;
         default:
-                fprintf(stderr, "ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
+                printf("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
                 return -EINVAL;
         }
 
-	printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud);
+	printf("[so3_comp_filt] UART is %s, baudrate is %d\n", uart_name, baud);
         uart = open(uart_name, O_RDWR | O_NOCTTY);
 
 	/* Try to set baud rate */
@@ -321,11 +331,11 @@ int open_uart(int baud, const char *uart_name, struct termios *uart_config_origi
         int termios_state;
         *is_usb = false;
 
-	       /* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */
+	/* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */
         if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) {
                 /* Back up the original uart configuration to restore it after exit */
                 if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-                        fprintf(stderr, "ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
+                        printf("ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
                         close(uart);
                         return -1;
                 }
@@ -338,14 +348,14 @@ int open_uart(int baud, const char *uart_name, struct termios *uart_config_origi
 
                 /* Set baud rate */
                 if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-                        fprintf(stderr, "ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+                        printf("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
                         close(uart);
                         return -1;
                 }
 
 
                 if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-                        fprintf(stderr, "ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
+                        printf("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
                         close(uart);
                         return -1;
                 }
@@ -420,9 +430,12 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	}
 
 	if(debug_mode){
+		printf("Opening debugging port for 3D visualization\n");
 		uart = open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
 		if (uart < 0)
                 	printf("could not open %s", device_name);
+		else
+			printf("Open port success\n");
 	}
 
 	// print text
@@ -638,30 +651,22 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
         				Rot_matrix[7] = 2.0 * (c * d - a * b);	// 32
         				Rot_matrix[8] = 2*aSq - 1 + 2*dSq;	// 33
 
+					gravity_vector[0] = 2*(q1*q3-q0*q2);
+					gravity_vector[1] = 2*(q0*q1+q2*q3);
+					gravity_vector[2] = aSq - bSq - cSq + dSq;
+
 					//euler[0] = atan2f(Rot_matrix[7], Rot_matrix[8]);
-					//euler[1] = asinf(-Rot_matrix[6]);
+					//euler[1] = -asinf(Rot_matrix[6]);
 					//euler[2] = atan2f(Rot_matrix[3],Rot_matrix[0]);
 
-					/* FIXME : Work around this later...
-					float theta = asinf(-Rot_matrix[6]);	// -r_{31}
-					euler[1] = theta;			// pitch angle
-
-					if(fabsf(theta - M_PI_2_F) < 1.0e-3f){
-						euler[0] = 0.0f;
-						euler[2] = atan2f(Rot_matrix[5] - Rot_matrix[1], Rot_matrix[2] + Rot_matrix[4] - euler[0]);
-					} else if (fabsf(theta + M_PI_2_F) < 1.0e-3f) {
-						euler[0] = 0.0f;
-						euler[2] = atan2f(Rot_matrix[5] - Rot_matrix[1], Rot_matrix[2] + Rot_matrix[4] - euler[0]);
-					} else {
-						euler[0] = atan2f(Rot_matrix[7], Rot_matrix[8]);
-						euler[2] = atan2f(Rot_matrix[3], Rot_matrix[0]);
-					}
-					*/
-
-					euler[2] = atan2f(2*(q1*q2-q0*q3),2*(q0*q0+q1*q1)-1);
-					euler[1]= -asinf(2*(q1*q3+q0*q2));
-					euler[0] = atan2f(2*(q2*q3-q0*q1),2*(q0*q0+q3*q3)-1);
-					
+					// Euler angle directly from quaternion.
+					euler[0] = -atan2f(gravity_vector[1], sqrtf(gravity_vector[0]*gravity_vector[0] + gravity_vector[2]*gravity_vector[2]));	// roll
+					euler[1] = atan2f(gravity_vector[0], sqrtf(gravity_vector[1]*gravity_vector[1] + gravity_vector[2]*gravity_vector[2]));	// pitch
+					euler[2] = -atan2f(2*(q1*q2-q0*q3),2*(q0*q0+q1*q1)-1);	// yaw
+				
+					//euler[2] = atan2(2 * q1*q2 - 2 * q0*q3, 2 * q0*q0 + 2 * q1*q1 - 1); // psi
+  					//euler[1] = -asin(2 * q1*q3 + 2 * q0*q2); // theta
+					//euler[0] = atan2(2 * q2*q3 - 2 * q0*q1, 2 * q0*q0 + 2 * q3*q3 - 1); // phi	
 
 					/* swap values for next iteration, check for fatal inputs */
 					if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
@@ -684,19 +689,15 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					att.yaw = euler[2] - so3_comp_params.yaw_off;
 
 					/* FIXME : This can be a problem for rate controller. Rate in body or inertial? */
-					att.rollspeed = q1;
-					att.pitchspeed = q2;
-					att.yawspeed = q3;
+					att.rollspeed = gyro[0];
+					att.pitchspeed = gyro[1];
+					att.yawspeed = gyro[2];
+					att.rollacc = 0;
+					att.pitchacc = 0;
+					att.yawacc = 0;
 
-					/*
-					att.rollacc = x_aposteriori[3];
-					att.pitchacc = x_aposteriori[4];
-					att.yawacc = x_aposteriori[5];
-					*/
-
-					//att.yawspeed =z_k[2] ;
-					/* copy offsets */
-					//memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
+					/* TODO: Bias estimation required */
+					memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets));
 
 					/* copy rotation matrix */
 					memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
@@ -712,6 +713,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 					perf_end(so3_comp_loop_perf);
 
+					//! This will print out debug packet to visualization software
 					if(debug_mode)
 					{
 						float quat[4];
@@ -722,12 +724,12 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						send_uart_float_arr(quat,4);
 						send_uart_byte('\n');
 					}
-				}// else
+				}
 			}
 		}
 
 		loopcounter++;
-	}
+	}// while
 
 	thread_running = false;