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
Revision 1:8a920397b510, committed 2010-09-06
- Comitter:
- aberk
- Date:
- Mon Sep 06 14:18:33 2010 +0000
- Parent:
- 0:976ab2e4e4bd
- Commit message:
- Added method to reset the filter.
Changed in this revision
IMUfilter.cpp | Show annotated file Show diff for this revision Revisions of this file |
IMUfilter.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r 976ab2e4e4bd -r 8a920397b510 IMUfilter.cpp --- 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; + +}
diff -r 976ab2e4e4bd -r 8a920397b510 IMUfilter.h --- a/IMUfilter.h Mon Sep 06 13:54:41 2010 +0000 +++ b/IMUfilter.h Mon Sep 06 14:18:33 2010 +0000 @@ -104,6 +104,11 @@ */ double getYaw(void); + /** + * Reset the filter. + */ + void reset(void); + private: int firstUpdate;