Diff: IMUfilter.cpp
- Revision:
- 1:4b0e8441f099
- Parent:
- 0:6a5296aa7dbf
- Child:
- 2:202906c5fadd
diff -r 6a5296aa7dbf -r 4b0e8441f099 IMUfilter.cpp --- 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)); }