Ian Molesworth / IMUfilter

IMUfilter.h@1:4b0e8441f099, 2011-02-07 (annotated)

Committer:
nimbusgb
Date:
Mon Feb 07 22:02:32 2011 +0000
Revision:
1:4b0e8441f099
Parent:
0:6a5296aa7dbf 
added ability to adjust gyro errors for tuning purposes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nimbusgb 1:4b0e8441f099 1 /**
nimbusgb 1:4b0e8441f099 2 * @author Aaron Berk
nimbusgb 1:4b0e8441f099 3 *
nimbusgb 1:4b0e8441f099 4 * @section LICENSE
nimbusgb 1:4b0e8441f099 5 *
nimbusgb 1:4b0e8441f099 6 * Copyright (c) 2010 ARM Limited
nimbusgb 1:4b0e8441f099 7 *
nimbusgb 1:4b0e8441f099 8 * Permission is hereby granted, free of charge, to any person obtaining a copy
nimbusgb 1:4b0e8441f099 9 * of this software and associated documentation files (the "Software"), to deal
nimbusgb 1:4b0e8441f099 10 * in the Software without restriction, including without limitation the rights
nimbusgb 1:4b0e8441f099 11 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
nimbusgb 1:4b0e8441f099 12 * copies of the Software, and to permit persons to whom the Software is
nimbusgb 1:4b0e8441f099 13 * furnished to do so, subject to the following conditions:
nimbusgb 1:4b0e8441f099 14 *
nimbusgb 1:4b0e8441f099 15 * The above copyright notice and this permission notice shall be included in
nimbusgb 1:4b0e8441f099 16 * all copies or substantial portions of the Software.
nimbusgb 1:4b0e8441f099 17 *
nimbusgb 1:4b0e8441f099 18 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
nimbusgb 1:4b0e8441f099 19 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
nimbusgb 1:4b0e8441f099 20 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
nimbusgb 1:4b0e8441f099 21 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
nimbusgb 1:4b0e8441f099 22 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
nimbusgb 1:4b0e8441f099 23 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
nimbusgb 1:4b0e8441f099 24 * THE SOFTWARE.
nimbusgb 1:4b0e8441f099 25 *
nimbusgb 1:4b0e8441f099 26 * @section DESCRIPTION
nimbusgb 1:4b0e8441f099 27 *
nimbusgb 1:4b0e8441f099 28 * IMU orientation filter developed by Sebastian Madgwick.
nimbusgb 1:4b0e8441f099 29 *
nimbusgb 1:4b0e8441f099 30 * Find more details about his paper here:
nimbusgb 1:4b0e8441f099 31 *
nimbusgb 1:4b0e8441f099 32 * http://code.google.com/p/imumargalgorithm30042010sohm/
nimbusgb 1:4b0e8441f099 33 */
nimbusgb 1:4b0e8441f099 34
nimbusgb 1:4b0e8441f099 35 #ifndef IMU_FILTER_H
nimbusgb 1:4b0e8441f099 36 #define IMU_FILTER_H
nimbusgb 1:4b0e8441f099 37
nimbusgb 1:4b0e8441f099 38 /**
nimbusgb 1:4b0e8441f099 39 * Includes
nimbusgb 1:4b0e8441f099 40 */
nimbusgb 1:4b0e8441f099 41 #include "mbed.h"
nimbusgb 1:4b0e8441f099 42
nimbusgb 1:4b0e8441f099 43 /**
nimbusgb 1:4b0e8441f099 44 * Defines
nimbusgb 1:4b0e8441f099 45 */
nimbusgb 1:4b0e8441f099 46 #define PI 3.1415926536
nimbusgb 1:4b0e8441f099 47
nimbusgb 1:4b0e8441f099 48 /**
nimbusgb 1:4b0e8441f099 49 * IMU orientation filter.
nimbusgb 1:4b0e8441f099 50 */
nimbusgb 1:4b0e8441f099 51 class IMUfilter {
nimbusgb 1:4b0e8441f099 52
nimbusgb 1:4b0e8441f099 53 public:
nimbusgb 1:4b0e8441f099 54
nimbusgb 1:4b0e8441f099 55 /**
nimbusgb 1:4b0e8441f099 56 * Constructor.
nimbusgb 1:4b0e8441f099 57 *
nimbusgb 1:4b0e8441f099 58 * Initializes filter variables.
nimbusgb 1:4b0e8441f099 59 *
nimbusgb 1:4b0e8441f099 60 * @param rate The rate at which the filter should be updated.
nimbusgb 1:4b0e8441f099 61 * @param gyroscopeMeasurementError The error of the gyroscope in degrees
nimbusgb 1:4b0e8441f099 62 * per second. This used to calculate a tuning constant for the filter.
nimbusgb 1:4b0e8441f099 63 * Try changing this value if there are jittery readings, or they change
nimbusgb 1:4b0e8441f099 64 * too much or too fast when rotating the IMU.
nimbusgb 1:4b0e8441f099 65 */
nimbusgb 1:4b0e8441f099 66 IMUfilter(double rate, double gyroscopeMeasurementError);
nimbusgb 1:4b0e8441f099 67
nimbusgb 1:4b0e8441f099 68 /**
nimbusgb 1:4b0e8441f099 69 * Update the filter variables.
nimbusgb 1:4b0e8441f099 70 *
nimbusgb 1:4b0e8441f099 71 * @param w_x X-axis gyroscope reading in rad/s.
nimbusgb 1:4b0e8441f099 72 * @param w_y Y-axis gyroscope reading in rad/s.
nimbusgb 1:4b0e8441f099 73 * @param w_z Z-axis gyroscope reading in rad/s.
nimbusgb 1:4b0e8441f099 74 * @param a_x X-axis accelerometer reading in m/s/s.
nimbusgb 1:4b0e8441f099 75 * @param a_y Y-axis accelerometer reading in m/s/s.
nimbusgb 1:4b0e8441f099 76 * @param a_z Z-axis accelerometer reading in m/s/s.
nimbusgb 1:4b0e8441f099 77 */
nimbusgb 1:4b0e8441f099 78 void updateFilter(double w_x, double w_y, double w_z,
nimbusgb 1:4b0e8441f099 79 double a_x, double a_y, double a_z);
nimbusgb 1:4b0e8441f099 80
nimbusgb 1:4b0e8441f099 81 /**
nimbusgb 1:4b0e8441f099 82 * Compute the Euler angles based on the current filter data.
nimbusgb 1:4b0e8441f099 83 */
nimbusgb 1:4b0e8441f099 84 void computeEuler(void);
nimbusgb 1:4b0e8441f099 85
nimbusgb 1:4b0e8441f099 86 /**
nimbusgb 1:4b0e8441f099 87 * Get the current roll.
nimbusgb 1:4b0e8441f099 88 *
nimbusgb 1:4b0e8441f099 89 * @return The current roll angle in radians.
nimbusgb 1:4b0e8441f099 90 */
nimbusgb 1:4b0e8441f099 91 double getRoll(void);
nimbusgb 1:4b0e8441f099 92
nimbusgb 1:4b0e8441f099 93 /**
nimbusgb 1:4b0e8441f099 94 * Get the current pitch.
nimbusgb 1:4b0e8441f099 95 *
nimbusgb 1:4b0e8441f099 96 * @return The current pitch angle in radians.
nimbusgb 1:4b0e8441f099 97 */
nimbusgb 1:4b0e8441f099 98 double getPitch(void);
nimbusgb 1:4b0e8441f099 99
nimbusgb 1:4b0e8441f099 100 /**
nimbusgb 1:4b0e8441f099 101 * Get the current yaw.
nimbusgb 1:4b0e8441f099 102 *
nimbusgb 1:4b0e8441f099 103 * @return The current yaw angle in radians.
nimbusgb 1:4b0e8441f099 104 */
nimbusgb 1:4b0e8441f099 105 double getYaw(void);
nimbusgb 1:4b0e8441f099 106
nimbusgb 1:4b0e8441f099 107 /**
nimbusgb 1:4b0e8441f099 108 * Get the current Gyro error rate.
nimbusgb 1:4b0e8441f099 109 *
nimbusgb 1:4b0e8441f099 110 * @return The Gyro error rate setting.
nimbusgb 1:4b0e8441f099 111 */
nimbusgb 1:4b0e8441f099 112 double getGyroError(void);
nimbusgb 1:4b0e8441f099 113
nimbusgb 1:4b0e8441f099 114 /**
nimbusgb 1:4b0e8441f099 115 * Set the Gyro error rate.
nimbusgb 1:4b0e8441f099 116 * The filter resets during calling this routine.
nimbusgb 1:4b0e8441f099 117 */
nimbusgb 1:4b0e8441f099 118 void setGyroError(double gyroscopeMeasurementError);
nimbusgb 1:4b0e8441f099 119
nimbusgb 1:4b0e8441f099 120 /**
nimbusgb 1:4b0e8441f099 121 * Reset the filter.
nimbusgb 1:4b0e8441f099 122 */
nimbusgb 1:4b0e8441f099 123 void reset(void);
nimbusgb 1:4b0e8441f099 124
nimbusgb 1:4b0e8441f099 125 private:
nimbusgb 1:4b0e8441f099 126
nimbusgb 1:4b0e8441f099 127 int firstUpdate;
nimbusgb 1:4b0e8441f099 128
nimbusgb 1:4b0e8441f099 129 //Quaternion orientation of earth frame relative to auxiliary frame.
nimbusgb 1:4b0e8441f099 130 double AEq_1;
nimbusgb 1:4b0e8441f099 131 double AEq_2;
nimbusgb 1:4b0e8441f099 132 double AEq_3;
nimbusgb 1:4b0e8441f099 133 double AEq_4;
nimbusgb 1:4b0e8441f099 134
nimbusgb 1:4b0e8441f099 135 //Estimated orientation quaternion elements with initial conditions.
nimbusgb 1:4b0e8441f099 136 double SEq_1;
nimbusgb 1:4b0e8441f099 137 double SEq_2;
nimbusgb 1:4b0e8441f099 138 double SEq_3;
nimbusgb 1:4b0e8441f099 139 double SEq_4;
nimbusgb 1:4b0e8441f099 140
nimbusgb 1:4b0e8441f099 141 //Sampling period
nimbusgb 1:4b0e8441f099 142 double deltat;
nimbusgb 1:4b0e8441f099 143
nimbusgb 1:4b0e8441f099 144 //Gyroscope measurement error (in degrees per second).
nimbusgb 1:4b0e8441f099 145 double gyroMeasError;
nimbusgb 1:4b0e8441f099 146
nimbusgb 1:4b0e8441f099 147 //Compute beta (filter tuning constant..
nimbusgb 1:4b0e8441f099 148 double beta;
nimbusgb 1:4b0e8441f099 149
nimbusgb 1:4b0e8441f099 150 double phi;
nimbusgb 1:4b0e8441f099 151 double theta;
nimbusgb 1:4b0e8441f099 152 double psi;
nimbusgb 1:4b0e8441f099 153
nimbusgb 1:4b0e8441f099 154 };
nimbusgb 1:4b0e8441f099 155
nimbusgb 1:4b0e8441f099 156 #endif /* IMU_FILTER_H */