AHRS based on MatrixPilot DCM algorithm; ported from Pololu MinIMU-9 example code in turn based on ArduPilot 1.5

Revision:
0:62284d27d75e
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Matrix.cpp	Tue Jan 24 17:40:40 2012 +0000
@@ -0,0 +1,54 @@
+void Vector_Cross_Product(float C[3], float A[3], float B[3])
+{
+    C[0] = (A[1] * B[2]) - (A[2] * B[1]);
+    C[1] = (A[2] * B[0]) - (A[0] * B[2]);
+    C[2] = (A[0] * B[1]) - (A[1] * B[0]);
+  
+    return;
+}
+
+void Vector_Scale(float C[3], float A[3], float b)
+{
+    for (int m = 0; m < 3; m++)
+        C[m] = A[m] * b;
+        
+    return;
+}
+
+float Vector_Dot_Product(float A[3], float B[3])
+{
+    float result = 0.0;
+
+    for (int i = 0; i < 3; i++) {
+        result += A[i] * B[i];
+    }
+    
+    return result;
+}
+
+void Vector_Add(float C[3], float A[3], float B[3])
+{
+    for (int m = 0; m < 3; m++)
+        C[m] = A[m] + B[m];
+        
+    return;
+}
+
+void Vector_Add(float C[3][3], float A[3][3], float B[3][3])
+{
+    for (int m = 0; m < 3; m++)
+        for (int n = 0; n < 3; n++)
+            C[m][n] = A[m][n] + B[m][n];
+}
+
+void Matrix_Multiply(float C[3][3], float A[3][3], float B[3][3])
+{
+    for (int i = 0; i < 3; i++) {
+        for (int j = 0; j < 3; j++) {
+            C[i][j] = 0;
+            for (int k = 0; k < 3; k++) {
+               C[i][j] += A[i][k] * B[k][j];
+            }
+        }
+    }
+}
\ No newline at end of file