Gal Yaroslavsky / MicroMouse-v1

asdf

Dependencies:   L3GD20 LSM303DLHC mbed

IMU_Filter/IMUFilter.cpp@8:ce5b1bf38077, 2014-04-03 (annotated)

Committer:
goy5022
Date:
Thu Apr 03 23:58:04 2014 +0000
Revision:
8:ce5b1bf38077
Parent:
1:cfe6a6ad8dca 
asdf

Who changed what in which revision?

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