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.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 }
Generated on Tue Jul 12 2022 21:11:53 by 1.7.2