Aaron Berk
Inertial measurement unit orientation filter. Ported from Sebastian Madgwick's paper, "An efficient orientation filter for inertial and inertial/magnetic sensor arrays".
Dependents: IMURover IMUfilter_HelloWorld IMUfilter_RPYExample 12_TCPIP ... more
Diff: IMUfilter.cpp
- Revision:
- 1:8a920397b510
- Parent:
- 0:976ab2e4e4bd
--- a/IMUfilter.cpp Mon Sep 06 13:54:41 2010 +0000
+++ b/IMUfilter.cpp Mon Sep 06 14:18:33 2010 +0000
@@ -208,3 +208,21 @@
return psi;
}
+
+void IMUfilter::reset(void) {
+
+ firstUpdate = 0;
+
+ //Quaternion orientation of earth frame relative to auxiliary frame.
+ AEq_1 = 1;
+ AEq_2 = 0;
+ AEq_3 = 0;
+ AEq_4 = 0;
+
+ //Estimated orientation quaternion elements with initial conditions.
+ SEq_1 = 1;
+ SEq_2 = 0;
+ SEq_3 = 0;
+ SEq_4 = 0;
+
+}