Diff: IMUfilter.cpp

Revision:

1:4b0e8441f099

Parent:

0:6a5296aa7dbf

Child:

2:202906c5fadd

--- a/IMUfilter.cpp	Sat Feb 05 18:57:55 2011 +0000
+++ b/IMUfilter.cpp	Mon Feb 07 22:02:32 2011 +0000
@@ -173,7 +173,7 @@
     double ASq_1, ASq_2, ASq_3, ASq_4;    
                               
     //Compute the quaternion conjugate.
-    ESq_1 = SEq_1;
+    ESq_1 =  SEq_1;
     ESq_2 = -SEq_2;
     ESq_3 = -SEq_3;
     ESq_4 = -SEq_4;
@@ -185,9 +185,9 @@
     ASq_4 = ESq_1 * AEq_4 + ESq_2 * AEq_3 - ESq_3 * AEq_2 + ESq_4 * AEq_1;
 
     //Compute the Euler angles from the quaternion.
-    phi = atan2(2 * ASq_3 * ASq_4 - 2 * ASq_1 * ASq_2, 2 * ASq_1 * ASq_1 + 2 * ASq_4 * ASq_4 - 1);
-    theta = asin(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_3);
-    psi = atan2(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_4, 2 * ASq_1 * ASq_1 + 2 * ASq_2 * ASq_2 - 1);
+    phi   = atan2(2 * ASq_3 * ASq_4 - 2 * ASq_1 * ASq_2, 2 * ASq_1 * ASq_1 + 2 * ASq_4 * ASq_4 - 1);
+    theta = asin (2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_3);
+    psi   = atan2(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_4, 2 * ASq_1 * ASq_1 + 2 * ASq_2 * ASq_2 - 1);
 
 }
 
@@ -209,6 +209,17 @@
 
 }
 
+void IMUfilter::setGyroError(double gyroscopeMeasurementError)
+    {
+    gyroMeasError = gyroscopeMeasurementError;
+    reset();
+    }
+    
+double IMUfilter::getGyroError(void)
+    {
+    return gyroMeasError;
+    }
+    
 void IMUfilter::reset(void) {
 
     firstUpdate = 0;
@@ -224,5 +235,7 @@
     SEq_2 = 0;
     SEq_3 = 0;
     SEq_4 = 0;
-
+    
+    //Compute beta.
+    beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0));
 }