Aaron Berk / IMUfilter

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

IMUfilter.h@1:8a920397b510, 2010-09-06 (annotated)

Committer:
aberk
Date:
Mon Sep 06 14:18:33 2010 +0000
Revision:
1:8a920397b510
Parent:
0:976ab2e4e4bd 
Added method to reset the filter.

Who changed what in which revision?

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