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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nimbusgb 0:6a5296aa7dbf 1 /**
nimbusgb 0:6a5296aa7dbf 2 * @author Aaron Berk
nimbusgb 0:6a5296aa7dbf 3 *
nimbusgb 0:6a5296aa7dbf 4 * @section LICENSE
nimbusgb 0:6a5296aa7dbf 5 *
nimbusgb 0:6a5296aa7dbf 6 * Copyright (c) 2010 ARM Limited
nimbusgb 0:6a5296aa7dbf 7 *
nimbusgb 0:6a5296aa7dbf 8 * Permission is hereby granted, free of charge, to any person obtaining a copy
nimbusgb 0:6a5296aa7dbf 9 * of this software and associated documentation files (the "Software"), to deal
nimbusgb 0:6a5296aa7dbf 10 * in the Software without restriction, including without limitation the rights
nimbusgb 0:6a5296aa7dbf 11 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
nimbusgb 0:6a5296aa7dbf 12 * copies of the Software, and to permit persons to whom the Software is
nimbusgb 0:6a5296aa7dbf 13 * furnished to do so, subject to the following conditions:
nimbusgb 0:6a5296aa7dbf 14 *
nimbusgb 0:6a5296aa7dbf 15 * The above copyright notice and this permission notice shall be included in
nimbusgb 0:6a5296aa7dbf 16 * all copies or substantial portions of the Software.
nimbusgb 0:6a5296aa7dbf 17 *
nimbusgb 0:6a5296aa7dbf 18 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
nimbusgb 0:6a5296aa7dbf 19 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
nimbusgb 0:6a5296aa7dbf 20 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
nimbusgb 0:6a5296aa7dbf 21 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
nimbusgb 0:6a5296aa7dbf 22 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
nimbusgb 0:6a5296aa7dbf 23 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
nimbusgb 0:6a5296aa7dbf 24 * THE SOFTWARE.
nimbusgb 0:6a5296aa7dbf 25 *
nimbusgb 0:6a5296aa7dbf 26 * @section DESCRIPTION
nimbusgb 0:6a5296aa7dbf 27 *
nimbusgb 0:6a5296aa7dbf 28 * IMU orientation filter developed by Sebastian Madgwick.
nimbusgb 0:6a5296aa7dbf 29 *
nimbusgb 0:6a5296aa7dbf 30 * Find more details about his paper here:
nimbusgb 0:6a5296aa7dbf 31 *
nimbusgb 0:6a5296aa7dbf 32 * http://code.google.com/p/imumargalgorithm30042010sohm/
nimbusgb 0:6a5296aa7dbf 33 */
nimbusgb 0:6a5296aa7dbf 34
nimbusgb 0:6a5296aa7dbf 35 /**
nimbusgb 0:6a5296aa7dbf 36 * Includes
nimbusgb 0:6a5296aa7dbf 37 */
nimbusgb 0:6a5296aa7dbf 38 #include "IMUfilter.h"
nimbusgb 0:6a5296aa7dbf 39
nimbusgb 0:6a5296aa7dbf 40 IMUfilter::IMUfilter(double rate, double gyroscopeMeasurementError){
nimbusgb 0:6a5296aa7dbf 41
nimbusgb 0:6a5296aa7dbf 42 firstUpdate = 0;
nimbusgb 0:6a5296aa7dbf 43
nimbusgb 0:6a5296aa7dbf 44 //Quaternion orientation of earth frame relative to auxiliary frame.
nimbusgb 0:6a5296aa7dbf 45 AEq_1 = 1;
nimbusgb 0:6a5296aa7dbf 46 AEq_2 = 0;
nimbusgb 0:6a5296aa7dbf 47 AEq_3 = 0;
nimbusgb 0:6a5296aa7dbf 48 AEq_4 = 0;
nimbusgb 0:6a5296aa7dbf 49
nimbusgb 0:6a5296aa7dbf 50 //Estimated orientation quaternion elements with initial conditions.
nimbusgb 0:6a5296aa7dbf 51 SEq_1 = 1;
nimbusgb 0:6a5296aa7dbf 52 SEq_2 = 0;
nimbusgb 0:6a5296aa7dbf 53 SEq_3 = 0;
nimbusgb 0:6a5296aa7dbf 54 SEq_4 = 0;
nimbusgb 0:6a5296aa7dbf 55
nimbusgb 0:6a5296aa7dbf 56 //Sampling period (typical value is ~0.1s).
nimbusgb 0:6a5296aa7dbf 57 deltat = rate;
nimbusgb 0:6a5296aa7dbf 58
nimbusgb 0:6a5296aa7dbf 59 //Gyroscope measurement error (in degrees per second).
nimbusgb 0:6a5296aa7dbf 60 gyroMeasError = gyroscopeMeasurementError;
nimbusgb 0:6a5296aa7dbf 61
nimbusgb 0:6a5296aa7dbf 62 //Compute beta.
nimbusgb 0:6a5296aa7dbf 63 beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0));
nimbusgb 0:6a5296aa7dbf 64
nimbusgb 0:6a5296aa7dbf 65 }
nimbusgb 0:6a5296aa7dbf 66
nimbusgb 0:6a5296aa7dbf 67 void IMUfilter::updateFilter(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z) {
nimbusgb 0:6a5296aa7dbf 68
nimbusgb 0:6a5296aa7dbf 69 //Local system variables.
nimbusgb 0:6a5296aa7dbf 70
nimbusgb 0:6a5296aa7dbf 71 //Vector norm.
nimbusgb 0:6a5296aa7dbf 72 double norm;
nimbusgb 0:6a5296aa7dbf 73 //Quaternion rate from gyroscope elements.
nimbusgb 0:6a5296aa7dbf 74 double SEqDot_omega_1;
nimbusgb 0:6a5296aa7dbf 75 double SEqDot_omega_2;
nimbusgb 0:6a5296aa7dbf 76 double SEqDot_omega_3;
nimbusgb 0:6a5296aa7dbf 77 double SEqDot_omega_4;
nimbusgb 0:6a5296aa7dbf 78 //Objective function elements.
nimbusgb 0:6a5296aa7dbf 79 double f_1;
nimbusgb 0:6a5296aa7dbf 80 double f_2;
nimbusgb 0:6a5296aa7dbf 81 double f_3;
nimbusgb 0:6a5296aa7dbf 82 //Objective function Jacobian elements.
nimbusgb 0:6a5296aa7dbf 83 double J_11or24;
nimbusgb 0:6a5296aa7dbf 84 double J_12or23;
nimbusgb 0:6a5296aa7dbf 85 double J_13or22;
nimbusgb 0:6a5296aa7dbf 86 double J_14or21;
nimbusgb 0:6a5296aa7dbf 87 double J_32;
nimbusgb 0:6a5296aa7dbf 88 double J_33;
nimbusgb 0:6a5296aa7dbf 89 //Objective function gradient elements.
nimbusgb 0:6a5296aa7dbf 90 double nablaf_1;
nimbusgb 0:6a5296aa7dbf 91 double nablaf_2;
nimbusgb 0:6a5296aa7dbf 92 double nablaf_3;
nimbusgb 0:6a5296aa7dbf 93 double nablaf_4;
nimbusgb 0:6a5296aa7dbf 94
nimbusgb 0:6a5296aa7dbf 95 //Auxiliary variables to avoid reapeated calculations.
nimbusgb 0:6a5296aa7dbf 96 double halfSEq_1 = 0.5 * SEq_1;
nimbusgb 0:6a5296aa7dbf 97 double halfSEq_2 = 0.5 * SEq_2;
nimbusgb 0:6a5296aa7dbf 98 double halfSEq_3 = 0.5 * SEq_3;
nimbusgb 0:6a5296aa7dbf 99 double halfSEq_4 = 0.5 * SEq_4;
nimbusgb 0:6a5296aa7dbf 100 double twoSEq_1 = 2.0 * SEq_1;
nimbusgb 0:6a5296aa7dbf 101 double twoSEq_2 = 2.0 * SEq_2;
nimbusgb 0:6a5296aa7dbf 102 double twoSEq_3 = 2.0 * SEq_3;
nimbusgb 0:6a5296aa7dbf 103
nimbusgb 0:6a5296aa7dbf 104 //Compute the quaternion rate measured by gyroscopes.
nimbusgb 0:6a5296aa7dbf 105 SEqDot_omega_1 = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z;
nimbusgb 0:6a5296aa7dbf 106 SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y;
nimbusgb 0:6a5296aa7dbf 107 SEqDot_omega_3 = halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x;
nimbusgb 0:6a5296aa7dbf 108 SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x;
nimbusgb 0:6a5296aa7dbf 109
nimbusgb 0:6a5296aa7dbf 110 //Normalise the accelerometer measurement.
nimbusgb 0:6a5296aa7dbf 111 norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
nimbusgb 0:6a5296aa7dbf 112 a_x /= norm;
nimbusgb 0:6a5296aa7dbf 113 a_y /= norm;
nimbusgb 0:6a5296aa7dbf 114 a_z /= norm;
nimbusgb 0:6a5296aa7dbf 115
nimbusgb 0:6a5296aa7dbf 116 //Compute the objective function and Jacobian.
nimbusgb 0:6a5296aa7dbf 117 f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x;
nimbusgb 0:6a5296aa7dbf 118 f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y;
nimbusgb 0:6a5296aa7dbf 119 f_3 = 1.0 - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z;
nimbusgb 0:6a5296aa7dbf 120 //J_11 negated in matrix multiplication.
nimbusgb 0:6a5296aa7dbf 121 J_11or24 = twoSEq_3;
nimbusgb 0:6a5296aa7dbf 122 J_12or23 = 2 * SEq_4;
nimbusgb 0:6a5296aa7dbf 123 //J_12 negated in matrix multiplication
nimbusgb 0:6a5296aa7dbf 124 J_13or22 = twoSEq_1;
nimbusgb 0:6a5296aa7dbf 125 J_14or21 = twoSEq_2;
nimbusgb 0:6a5296aa7dbf 126 //Negated in matrix multiplication.
nimbusgb 0:6a5296aa7dbf 127 J_32 = 2 * J_14or21;
nimbusgb 0:6a5296aa7dbf 128 //Negated in matrix multiplication.
nimbusgb 0:6a5296aa7dbf 129 J_33 = 2 * J_11or24;
nimbusgb 0:6a5296aa7dbf 130
nimbusgb 0:6a5296aa7dbf 131 //Compute the gradient (matrix multiplication).
nimbusgb 0:6a5296aa7dbf 132 nablaf_1 = J_14or21 * f_2 - J_11or24 * f_1;
nimbusgb 0:6a5296aa7dbf 133 nablaf_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3;
nimbusgb 0:6a5296aa7dbf 134 nablaf_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1;
nimbusgb 0:6a5296aa7dbf 135 nablaf_4 = J_14or21 * f_1 + J_11or24 * f_2;
nimbusgb 0:6a5296aa7dbf 136
nimbusgb 0:6a5296aa7dbf 137 //Normalise the gradient.
nimbusgb 0:6a5296aa7dbf 138 norm = sqrt(nablaf_1 * nablaf_1 + nablaf_2 * nablaf_2 + nablaf_3 * nablaf_3 + nablaf_4 * nablaf_4);
nimbusgb 0:6a5296aa7dbf 139 nablaf_1 /= norm;
nimbusgb 0:6a5296aa7dbf 140 nablaf_2 /= norm;
nimbusgb 0:6a5296aa7dbf 141 nablaf_3 /= norm;
nimbusgb 0:6a5296aa7dbf 142 nablaf_4 /= norm;
nimbusgb 0:6a5296aa7dbf 143
nimbusgb 0:6a5296aa7dbf 144 //Compute then integrate the estimated quaternion rate.
nimbusgb 0:6a5296aa7dbf 145 SEq_1 += (SEqDot_omega_1 - (beta * nablaf_1)) * deltat;
nimbusgb 0:6a5296aa7dbf 146 SEq_2 += (SEqDot_omega_2 - (beta * nablaf_2)) * deltat;
nimbusgb 0:6a5296aa7dbf 147 SEq_3 += (SEqDot_omega_3 - (beta * nablaf_3)) * deltat;
nimbusgb 0:6a5296aa7dbf 148 SEq_4 += (SEqDot_omega_4 - (beta * nablaf_4)) * deltat;
nimbusgb 0:6a5296aa7dbf 149
nimbusgb 0:6a5296aa7dbf 150 //Normalise quaternion
nimbusgb 0:6a5296aa7dbf 151 norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
nimbusgb 0:6a5296aa7dbf 152 SEq_1 /= norm;
nimbusgb 0:6a5296aa7dbf 153 SEq_2 /= norm;
nimbusgb 0:6a5296aa7dbf 154 SEq_3 /= norm;
nimbusgb 0:6a5296aa7dbf 155 SEq_4 /= norm;
nimbusgb 0:6a5296aa7dbf 156
nimbusgb 0:6a5296aa7dbf 157 if (firstUpdate == 0) {
nimbusgb 0:6a5296aa7dbf 158 //Store orientation of auxiliary frame.
nimbusgb 0:6a5296aa7dbf 159 AEq_1 = SEq_1;
nimbusgb 0:6a5296aa7dbf 160 AEq_2 = SEq_2;
nimbusgb 0:6a5296aa7dbf 161 AEq_3 = SEq_3;
nimbusgb 0:6a5296aa7dbf 162 AEq_4 = SEq_4;
nimbusgb 0:6a5296aa7dbf 163 firstUpdate = 1;
nimbusgb 0:6a5296aa7dbf 164 }
nimbusgb 0:6a5296aa7dbf 165
nimbusgb 0:6a5296aa7dbf 166 }
nimbusgb 0:6a5296aa7dbf 167
nimbusgb 0:6a5296aa7dbf 168 void IMUfilter::computeEuler(void){
nimbusgb 0:6a5296aa7dbf 169
nimbusgb 0:6a5296aa7dbf 170 //Quaternion describing orientation of sensor relative to earth.
nimbusgb 0:6a5296aa7dbf 171 double ESq_1, ESq_2, ESq_3, ESq_4;
nimbusgb 0:6a5296aa7dbf 172 //Quaternion describing orientation of sensor relative to auxiliary frame.
nimbusgb 0:6a5296aa7dbf 173 double ASq_1, ASq_2, ASq_3, ASq_4;
nimbusgb 0:6a5296aa7dbf 174
nimbusgb 0:6a5296aa7dbf 175 //Compute the quaternion conjugate.
nimbusgb 1:4b0e8441f099 176 ESq_1 = SEq_1;
nimbusgb 0:6a5296aa7dbf 177 ESq_2 = -SEq_2;
nimbusgb 0:6a5296aa7dbf 178 ESq_3 = -SEq_3;
nimbusgb 0:6a5296aa7dbf 179 ESq_4 = -SEq_4;
nimbusgb 0:6a5296aa7dbf 180
nimbusgb 0:6a5296aa7dbf 181 //Compute the quaternion product.
nimbusgb 0:6a5296aa7dbf 182 ASq_1 = ESq_1 * AEq_1 - ESq_2 * AEq_2 - ESq_3 * AEq_3 - ESq_4 * AEq_4;
nimbusgb 0:6a5296aa7dbf 183 ASq_2 = ESq_1 * AEq_2 + ESq_2 * AEq_1 + ESq_3 * AEq_4 - ESq_4 * AEq_3;
nimbusgb 0:6a5296aa7dbf 184 ASq_3 = ESq_1 * AEq_3 - ESq_2 * AEq_4 + ESq_3 * AEq_1 + ESq_4 * AEq_2;
nimbusgb 0:6a5296aa7dbf 185 ASq_4 = ESq_1 * AEq_4 + ESq_2 * AEq_3 - ESq_3 * AEq_2 + ESq_4 * AEq_1;
nimbusgb 0:6a5296aa7dbf 186
nimbusgb 0:6a5296aa7dbf 187 //Compute the Euler angles from the quaternion.
nimbusgb 1:4b0e8441f099 188 phi = atan2(2 * ASq_3 * ASq_4 - 2 * ASq_1 * ASq_2, 2 * ASq_1 * ASq_1 + 2 * ASq_4 * ASq_4 - 1);
nimbusgb 1:4b0e8441f099 189 theta = asin (2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_3);
nimbusgb 1:4b0e8441f099 190 psi = atan2(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_4, 2 * ASq_1 * ASq_1 + 2 * ASq_2 * ASq_2 - 1);
nimbusgb 0:6a5296aa7dbf 191
nimbusgb 0:6a5296aa7dbf 192 }
nimbusgb 0:6a5296aa7dbf 193
nimbusgb 0:6a5296aa7dbf 194 double IMUfilter::getRoll(void){
nimbusgb 0:6a5296aa7dbf 195
nimbusgb 0:6a5296aa7dbf 196 return phi;
nimbusgb 0:6a5296aa7dbf 197
nimbusgb 0:6a5296aa7dbf 198 }
nimbusgb 0:6a5296aa7dbf 199
nimbusgb 0:6a5296aa7dbf 200 double IMUfilter::getPitch(void){
nimbusgb 0:6a5296aa7dbf 201
nimbusgb 0:6a5296aa7dbf 202 return theta;
nimbusgb 0:6a5296aa7dbf 203
nimbusgb 0:6a5296aa7dbf 204 }
nimbusgb 0:6a5296aa7dbf 205
nimbusgb 0:6a5296aa7dbf 206 double IMUfilter::getYaw(void){
nimbusgb 0:6a5296aa7dbf 207
nimbusgb 0:6a5296aa7dbf 208 return psi;
nimbusgb 0:6a5296aa7dbf 209
nimbusgb 0:6a5296aa7dbf 210 }
nimbusgb 0:6a5296aa7dbf 211
nimbusgb 1:4b0e8441f099 212 void IMUfilter::setGyroError(double gyroscopeMeasurementError)
nimbusgb 1:4b0e8441f099 213 {
nimbusgb 1:4b0e8441f099 214 gyroMeasError = gyroscopeMeasurementError;
nimbusgb 1:4b0e8441f099 215 reset();
nimbusgb 1:4b0e8441f099 216 }
nimbusgb 1:4b0e8441f099 217
nimbusgb 1:4b0e8441f099 218 double IMUfilter::getGyroError(void)
nimbusgb 1:4b0e8441f099 219 {
nimbusgb 1:4b0e8441f099 220 return gyroMeasError;
nimbusgb 1:4b0e8441f099 221 }
nimbusgb 1:4b0e8441f099 222
nimbusgb 0:6a5296aa7dbf 223 void IMUfilter::reset(void) {
nimbusgb 0:6a5296aa7dbf 224
nimbusgb 0:6a5296aa7dbf 225 firstUpdate = 0;
nimbusgb 0:6a5296aa7dbf 226
nimbusgb 0:6a5296aa7dbf 227 //Quaternion orientation of earth frame relative to auxiliary frame.
nimbusgb 0:6a5296aa7dbf 228 AEq_1 = 1;
nimbusgb 0:6a5296aa7dbf 229 AEq_2 = 0;
nimbusgb 0:6a5296aa7dbf 230 AEq_3 = 0;
nimbusgb 0:6a5296aa7dbf 231 AEq_4 = 0;
nimbusgb 0:6a5296aa7dbf 232
nimbusgb 0:6a5296aa7dbf 233 //Estimated orientation quaternion elements with initial conditions.
nimbusgb 0:6a5296aa7dbf 234 SEq_1 = 1;
nimbusgb 0:6a5296aa7dbf 235 SEq_2 = 0;
nimbusgb 0:6a5296aa7dbf 236 SEq_3 = 0;
nimbusgb 0:6a5296aa7dbf 237 SEq_4 = 0;
nimbusgb 1:4b0e8441f099 238
nimbusgb 1:4b0e8441f099 239 //Compute beta.
nimbusgb 1:4b0e8441f099 240 beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0));
nimbusgb 0:6a5296aa7dbf 241 }