IMUfilter.h

00001 /**
00002  * @author Aaron Berk
00003  *
00004  * @section LICENSE
00005  *
00006  * Copyright (c) 2010 ARM Limited
00007  *
00008  * Permission is hereby granted, free of charge, to any person obtaining a copy
00009  * of this software and associated documentation files (the "Software"), to deal
00010  * in the Software without restriction, including without limitation the rights
00011  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00012  * copies of the Software, and to permit persons to whom the Software is
00013  * furnished to do so, subject to the following conditions:
00014  *
00015  * The above copyright notice and this permission notice shall be included in
00016  * all copies or substantial portions of the Software.
00017  *
00018  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00019  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00020  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00021  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00022  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00023  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00024  * THE SOFTWARE.
00025  *
00026  * @section DESCRIPTION
00027  *
00028  * IMU orientation filter developed by Sebastian Madgwick.
00029  *
00030  * Find more details about his paper here:
00031  *
00032  * http://code.google.com/p/imumargalgorithm30042010sohm/
00033  */
00034 
00035 #ifndef IMU_FILTER_H
00036 #define IMU_FILTER_H
00037 
00038 /**
00039  * Includes
00040  */
00041 #include "mbed.h"
00042 
00043 /**
00044  * Defines
00045  */
00046 #define PI 3.1415926536
00047 
00048 /**
00049  * IMU orientation filter.
00050  */
00051 class IMUfilter {
00052 
00053 public:
00054 
00055     /**
00056      * Constructor.
00057      *
00058      * Initializes filter variables.
00059      *
00060      * @param rate The rate at which the filter should be updated.
00061      * @param gyroscopeMeasurementError The error of the gyroscope in degrees
00062      *  per second. This used to calculate a tuning constant for the filter.
00063      *  Try changing this value if there are jittery readings, or they change
00064      *  too much or too fast when rotating the IMU.
00065      */
00066     IMUfilter(double rate, double gyroscopeMeasurementError);
00067 
00068     /**
00069      * Update the filter variables.
00070      *
00071      * @param w_x X-axis gyroscope reading in rad/s.
00072      * @param w_y Y-axis gyroscope reading in rad/s.
00073      * @param w_z Z-axis gyroscope reading in rad/s.
00074      * @param a_x X-axis accelerometer reading in m/s/s.
00075      * @param a_y Y-axis accelerometer reading in m/s/s.
00076      * @param a_z Z-axis accelerometer reading in m/s/s.
00077      */
00078     void updateFilter(double w_x, double w_y, double w_z,
00079                       double a_x, double a_y, double a_z);
00080 
00081     /**
00082      * Compute the Euler angles based on the current filter data.
00083      */
00084     void computeEuler(void);
00085 
00086     /**
00087      * Get the current roll.
00088      *
00089      * @return The current roll angle in radians.
00090      */
00091     double getRoll(void);
00092 
00093     /**
00094      * Get the current pitch.
00095      *
00096      * @return The current pitch angle in radians.
00097      */
00098     double getPitch(void);
00099 
00100     /**
00101      * Get the current yaw.
00102      *
00103      * @return The current yaw angle in radians.
00104      */
00105     double getYaw(void);
00106 
00107     /**
00108      * Reset the filter.
00109      */
00110     void reset(void);
00111 
00112 private:
00113 
00114     int firstUpdate;
00115 
00116     //Quaternion orientation of earth frame relative to auxiliary frame.
00117     double AEq_1;
00118     double AEq_2;
00119     double AEq_3;
00120     double AEq_4;
00121 
00122     //Estimated orientation quaternion elements with initial conditions.
00123     double SEq_1;
00124     double SEq_2;
00125     double SEq_3;
00126     double SEq_4;
00127 
00128     //Sampling period
00129     double deltat;
00130 
00131     //Gyroscope measurement error (in degrees per second).
00132     double gyroMeasError;
00133 
00134     //Compute beta (filter tuning constant..
00135     double beta;
00136 
00137     double phi;
00138     double theta;
00139     double psi;
00140 
00141 };
00142 
00143 #endif /* IMU_FILTER_H */