Revision:
1:4b0e8441f099
Parent:
0:6a5296aa7dbf
Child:
```--- 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));
}
```