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

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers IMUfilter.cpp Source File

IMUfilter.cpp

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 /**
00036  * Includes
00037  */
00038 #include "IMUfilter.h"
00039 
00040 IMUfilter::IMUfilter(double rate, double gyroscopeMeasurementError){
00041 
00042     firstUpdate = 0;
00043     
00044     //Quaternion orientation of earth frame relative to auxiliary frame.
00045     AEq_1 = 1;
00046     AEq_2 = 0;
00047     AEq_3 = 0;
00048     AEq_4 = 0;
00049     
00050     //Estimated orientation quaternion elements with initial conditions.
00051     SEq_1 = 1;
00052     SEq_2 = 0;
00053     SEq_3 = 0;
00054     SEq_4 = 0;
00055 
00056     //Sampling period (typical value is ~0.1s).
00057     deltat = rate;
00058     
00059     //Gyroscope measurement error (in degrees per second).
00060     gyroMeasError = gyroscopeMeasurementError;
00061     
00062     //Compute beta.
00063     beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0));
00064 
00065 }
00066 
00067 void IMUfilter::updateFilter(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z) {
00068 
00069     //Local system variables.
00070 
00071     //Vector norm.
00072     double norm;
00073     //Quaternion rate from gyroscope elements.
00074     double SEqDot_omega_1;
00075     double SEqDot_omega_2;
00076     double SEqDot_omega_3;
00077     double SEqDot_omega_4;
00078     //Objective function elements.
00079     double f_1;
00080     double f_2;
00081     double f_3;
00082     //Objective function Jacobian elements.
00083     double J_11or24;
00084     double J_12or23;
00085     double J_13or22;
00086     double J_14or21;
00087     double J_32;
00088     double J_33;
00089     //Objective function gradient elements.
00090     double nablaf_1;
00091     double nablaf_2;
00092     double nablaf_3;
00093     double nablaf_4;
00094 
00095     //Auxiliary variables to avoid reapeated calcualtions.
00096     double halfSEq_1 = 0.5 * SEq_1;
00097     double halfSEq_2 = 0.5 * SEq_2;
00098     double halfSEq_3 = 0.5 * SEq_3;
00099     double halfSEq_4 = 0.5 * SEq_4;
00100     double twoSEq_1 = 2.0 * SEq_1;
00101     double twoSEq_2 = 2.0 * SEq_2;
00102     double twoSEq_3 = 2.0 * SEq_3;
00103 
00104     //Compute the quaternion rate measured by gyroscopes.
00105     SEqDot_omega_1 = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z;
00106     SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y;
00107     SEqDot_omega_3 = halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x;
00108     SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x;
00109 
00110     //Normalise the accelerometer measurement.
00111     norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
00112     a_x /= norm;
00113     a_y /= norm;
00114     a_z /= norm;
00115 
00116     //Compute the objective function and Jacobian.
00117     f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x;
00118     f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y;
00119     f_3 = 1.0 - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z;
00120     //J_11 negated in matrix multiplication.
00121     J_11or24 = twoSEq_3;
00122     J_12or23 = 2 * SEq_4;
00123     //J_12 negated in matrix multiplication
00124     J_13or22 = twoSEq_1;
00125     J_14or21 = twoSEq_2;
00126     //Negated in matrix multiplication.
00127     J_32 = 2 * J_14or21;
00128     //Negated in matrix multiplication.
00129     J_33 = 2 * J_11or24;
00130 
00131     //Compute the gradient (matrix multiplication).
00132     nablaf_1 = J_14or21 * f_2 - J_11or24 * f_1;
00133     nablaf_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3;
00134     nablaf_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1;
00135     nablaf_4 = J_14or21 * f_1 + J_11or24 * f_2;
00136 
00137     //Normalise the gradient.
00138     norm = sqrt(nablaf_1 * nablaf_1 + nablaf_2 * nablaf_2 + nablaf_3 * nablaf_3 + nablaf_4 * nablaf_4);
00139     nablaf_1 /= norm;
00140     nablaf_2 /= norm;
00141     nablaf_3 /= norm;
00142     nablaf_4 /= norm;
00143 
00144     //Compute then integrate the estimated quaternion rate.
00145     SEq_1 += (SEqDot_omega_1 - (beta * nablaf_1)) * deltat;
00146     SEq_2 += (SEqDot_omega_2 - (beta * nablaf_2)) * deltat;
00147     SEq_3 += (SEqDot_omega_3 - (beta * nablaf_3)) * deltat;
00148     SEq_4 += (SEqDot_omega_4 - (beta * nablaf_4)) * deltat;
00149 
00150     //Normalise quaternion
00151     norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
00152     SEq_1 /= norm;
00153     SEq_2 /= norm;
00154     SEq_3 /= norm;
00155     SEq_4 /= norm;
00156 
00157     if (firstUpdate == 0) {
00158         //Store orientation of auxiliary frame.
00159         AEq_1 = SEq_1;
00160         AEq_2 = SEq_2;
00161         AEq_3 = SEq_3;
00162         AEq_4 = SEq_4;
00163         firstUpdate = 1;
00164     }
00165 
00166 }
00167 
00168 void IMUfilter::computeEuler(void){
00169 
00170     //Quaternion describing orientation of sensor relative to earth.
00171     double ESq_1, ESq_2, ESq_3, ESq_4;
00172     //Quaternion describing orientation of sensor relative to auxiliary frame.
00173     double ASq_1, ASq_2, ASq_3, ASq_4;    
00174                               
00175     //Compute the quaternion conjugate.
00176     ESq_1 = SEq_1;
00177     ESq_2 = -SEq_2;
00178     ESq_3 = -SEq_3;
00179     ESq_4 = -SEq_4;
00180 
00181     //Compute the quaternion product.
00182     ASq_1 = ESq_1 * AEq_1 - ESq_2 * AEq_2 - ESq_3 * AEq_3 - ESq_4 * AEq_4;
00183     ASq_2 = ESq_1 * AEq_2 + ESq_2 * AEq_1 + ESq_3 * AEq_4 - ESq_4 * AEq_3;
00184     ASq_3 = ESq_1 * AEq_3 - ESq_2 * AEq_4 + ESq_3 * AEq_1 + ESq_4 * AEq_2;
00185     ASq_4 = ESq_1 * AEq_4 + ESq_2 * AEq_3 - ESq_3 * AEq_2 + ESq_4 * AEq_1;
00186 
00187     //Compute the Euler angles from the quaternion.
00188     phi = atan2(2 * ASq_3 * ASq_4 - 2 * ASq_1 * ASq_2, 2 * ASq_1 * ASq_1 + 2 * ASq_4 * ASq_4 - 1);
00189     theta = asin(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_3);
00190     psi = atan2(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_4, 2 * ASq_1 * ASq_1 + 2 * ASq_2 * ASq_2 - 1);
00191 
00192 }
00193 
00194 double IMUfilter::getRoll(void){
00195 
00196     return phi;
00197 
00198 }
00199 
00200 double IMUfilter::getPitch(void){
00201 
00202     return theta;
00203 
00204 }
00205 
00206 double IMUfilter::getYaw(void){
00207 
00208     return psi;
00209 
00210 }
00211 
00212 void IMUfilter::reset(void) {
00213 
00214     firstUpdate = 0;
00215 
00216     //Quaternion orientation of earth frame relative to auxiliary frame.
00217     AEq_1 = 1;
00218     AEq_2 = 0;
00219     AEq_3 = 0;
00220     AEq_4 = 0;
00221 
00222     //Estimated orientation quaternion elements with initial conditions.
00223     SEq_1 = 1;
00224     SEq_2 = 0;
00225     SEq_3 = 0;
00226     SEq_4 = 0;
00227 
00228 }