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.cpp@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 /**
aberk 0:976ab2e4e4bd 36 * Includes
aberk 0:976ab2e4e4bd 37 */
aberk 0:976ab2e4e4bd 38 #include "IMUfilter.h"
aberk 0:976ab2e4e4bd 39
aberk 0:976ab2e4e4bd 40 IMUfilter::IMUfilter(double rate, double gyroscopeMeasurementError){
aberk 0:976ab2e4e4bd 41
aberk 0:976ab2e4e4bd 42 firstUpdate = 0;
aberk 0:976ab2e4e4bd 43
aberk 0:976ab2e4e4bd 44 //Quaternion orientation of earth frame relative to auxiliary frame.
aberk 0:976ab2e4e4bd 45 AEq_1 = 1;
aberk 0:976ab2e4e4bd 46 AEq_2 = 0;
aberk 0:976ab2e4e4bd 47 AEq_3 = 0;
aberk 0:976ab2e4e4bd 48 AEq_4 = 0;
aberk 0:976ab2e4e4bd 49
aberk 0:976ab2e4e4bd 50 //Estimated orientation quaternion elements with initial conditions.
aberk 0:976ab2e4e4bd 51 SEq_1 = 1;
aberk 0:976ab2e4e4bd 52 SEq_2 = 0;
aberk 0:976ab2e4e4bd 53 SEq_3 = 0;
aberk 0:976ab2e4e4bd 54 SEq_4 = 0;
aberk 0:976ab2e4e4bd 55
aberk 0:976ab2e4e4bd 56 //Sampling period (typical value is ~0.1s).
aberk 0:976ab2e4e4bd 57 deltat = rate;
aberk 0:976ab2e4e4bd 58
aberk 0:976ab2e4e4bd 59 //Gyroscope measurement error (in degrees per second).
aberk 0:976ab2e4e4bd 60 gyroMeasError = gyroscopeMeasurementError;
aberk 0:976ab2e4e4bd 61
aberk 0:976ab2e4e4bd 62 //Compute beta.
aberk 0:976ab2e4e4bd 63 beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0));
aberk 0:976ab2e4e4bd 64
aberk 0:976ab2e4e4bd 65 }
aberk 0:976ab2e4e4bd 66
aberk 0:976ab2e4e4bd 67 void IMUfilter::updateFilter(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z) {
aberk 0:976ab2e4e4bd 68
aberk 0:976ab2e4e4bd 69 //Local system variables.
aberk 0:976ab2e4e4bd 70
aberk 0:976ab2e4e4bd 71 //Vector norm.
aberk 0:976ab2e4e4bd 72 double norm;
aberk 0:976ab2e4e4bd 73 //Quaternion rate from gyroscope elements.
aberk 0:976ab2e4e4bd 74 double SEqDot_omega_1;
aberk 0:976ab2e4e4bd 75 double SEqDot_omega_2;
aberk 0:976ab2e4e4bd 76 double SEqDot_omega_3;
aberk 0:976ab2e4e4bd 77 double SEqDot_omega_4;
aberk 0:976ab2e4e4bd 78 //Objective function elements.
aberk 0:976ab2e4e4bd 79 double f_1;
aberk 0:976ab2e4e4bd 80 double f_2;
aberk 0:976ab2e4e4bd 81 double f_3;
aberk 0:976ab2e4e4bd 82 //Objective function Jacobian elements.
aberk 0:976ab2e4e4bd 83 double J_11or24;
aberk 0:976ab2e4e4bd 84 double J_12or23;
aberk 0:976ab2e4e4bd 85 double J_13or22;
aberk 0:976ab2e4e4bd 86 double J_14or21;
aberk 0:976ab2e4e4bd 87 double J_32;
aberk 0:976ab2e4e4bd 88 double J_33;
aberk 0:976ab2e4e4bd 89 //Objective function gradient elements.
aberk 0:976ab2e4e4bd 90 double nablaf_1;
aberk 0:976ab2e4e4bd 91 double nablaf_2;
aberk 0:976ab2e4e4bd 92 double nablaf_3;
aberk 0:976ab2e4e4bd 93 double nablaf_4;
aberk 0:976ab2e4e4bd 94
aberk 0:976ab2e4e4bd 95 //Auxiliary variables to avoid reapeated calcualtions.
aberk 0:976ab2e4e4bd 96 double halfSEq_1 = 0.5 * SEq_1;
aberk 0:976ab2e4e4bd 97 double halfSEq_2 = 0.5 * SEq_2;
aberk 0:976ab2e4e4bd 98 double halfSEq_3 = 0.5 * SEq_3;
aberk 0:976ab2e4e4bd 99 double halfSEq_4 = 0.5 * SEq_4;
aberk 0:976ab2e4e4bd 100 double twoSEq_1 = 2.0 * SEq_1;
aberk 0:976ab2e4e4bd 101 double twoSEq_2 = 2.0 * SEq_2;
aberk 0:976ab2e4e4bd 102 double twoSEq_3 = 2.0 * SEq_3;
aberk 0:976ab2e4e4bd 103
aberk 0:976ab2e4e4bd 104 //Compute the quaternion rate measured by gyroscopes.
aberk 0:976ab2e4e4bd 105 SEqDot_omega_1 = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z;
aberk 0:976ab2e4e4bd 106 SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y;
aberk 0:976ab2e4e4bd 107 SEqDot_omega_3 = halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x;
aberk 0:976ab2e4e4bd 108 SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x;
aberk 0:976ab2e4e4bd 109
aberk 0:976ab2e4e4bd 110 //Normalise the accelerometer measurement.
aberk 0:976ab2e4e4bd 111 norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
aberk 0:976ab2e4e4bd 112 a_x /= norm;
aberk 0:976ab2e4e4bd 113 a_y /= norm;
aberk 0:976ab2e4e4bd 114 a_z /= norm;
aberk 0:976ab2e4e4bd 115
aberk 0:976ab2e4e4bd 116 //Compute the objective function and Jacobian.
aberk 0:976ab2e4e4bd 117 f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x;
aberk 0:976ab2e4e4bd 118 f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y;
aberk 0:976ab2e4e4bd 119 f_3 = 1.0 - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z;
aberk 0:976ab2e4e4bd 120 //J_11 negated in matrix multiplication.
aberk 0:976ab2e4e4bd 121 J_11or24 = twoSEq_3;
aberk 0:976ab2e4e4bd 122 J_12or23 = 2 * SEq_4;
aberk 0:976ab2e4e4bd 123 //J_12 negated in matrix multiplication
aberk 0:976ab2e4e4bd 124 J_13or22 = twoSEq_1;
aberk 0:976ab2e4e4bd 125 J_14or21 = twoSEq_2;
aberk 0:976ab2e4e4bd 126 //Negated in matrix multiplication.
aberk 0:976ab2e4e4bd 127 J_32 = 2 * J_14or21;
aberk 0:976ab2e4e4bd 128 //Negated in matrix multiplication.
aberk 0:976ab2e4e4bd 129 J_33 = 2 * J_11or24;
aberk 0:976ab2e4e4bd 130
aberk 0:976ab2e4e4bd 131 //Compute the gradient (matrix multiplication).
aberk 0:976ab2e4e4bd 132 nablaf_1 = J_14or21 * f_2 - J_11or24 * f_1;
aberk 0:976ab2e4e4bd 133 nablaf_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3;
aberk 0:976ab2e4e4bd 134 nablaf_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1;
aberk 0:976ab2e4e4bd 135 nablaf_4 = J_14or21 * f_1 + J_11or24 * f_2;
aberk 0:976ab2e4e4bd 136
aberk 0:976ab2e4e4bd 137 //Normalise the gradient.
aberk 0:976ab2e4e4bd 138 norm = sqrt(nablaf_1 * nablaf_1 + nablaf_2 * nablaf_2 + nablaf_3 * nablaf_3 + nablaf_4 * nablaf_4);
aberk 0:976ab2e4e4bd 139 nablaf_1 /= norm;
aberk 0:976ab2e4e4bd 140 nablaf_2 /= norm;
aberk 0:976ab2e4e4bd 141 nablaf_3 /= norm;
aberk 0:976ab2e4e4bd 142 nablaf_4 /= norm;
aberk 0:976ab2e4e4bd 143
aberk 0:976ab2e4e4bd 144 //Compute then integrate the estimated quaternion rate.
aberk 0:976ab2e4e4bd 145 SEq_1 += (SEqDot_omega_1 - (beta * nablaf_1)) * deltat;
aberk 0:976ab2e4e4bd 146 SEq_2 += (SEqDot_omega_2 - (beta * nablaf_2)) * deltat;
aberk 0:976ab2e4e4bd 147 SEq_3 += (SEqDot_omega_3 - (beta * nablaf_3)) * deltat;
aberk 0:976ab2e4e4bd 148 SEq_4 += (SEqDot_omega_4 - (beta * nablaf_4)) * deltat;
aberk 0:976ab2e4e4bd 149
aberk 0:976ab2e4e4bd 150 //Normalise quaternion
aberk 0:976ab2e4e4bd 151 norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
aberk 0:976ab2e4e4bd 152 SEq_1 /= norm;
aberk 0:976ab2e4e4bd 153 SEq_2 /= norm;
aberk 0:976ab2e4e4bd 154 SEq_3 /= norm;
aberk 0:976ab2e4e4bd 155 SEq_4 /= norm;
aberk 0:976ab2e4e4bd 156
aberk 0:976ab2e4e4bd 157 if (firstUpdate == 0) {
aberk 0:976ab2e4e4bd 158 //Store orientation of auxiliary frame.
aberk 0:976ab2e4e4bd 159 AEq_1 = SEq_1;
aberk 0:976ab2e4e4bd 160 AEq_2 = SEq_2;
aberk 0:976ab2e4e4bd 161 AEq_3 = SEq_3;
aberk 0:976ab2e4e4bd 162 AEq_4 = SEq_4;
aberk 0:976ab2e4e4bd 163 firstUpdate = 1;
aberk 0:976ab2e4e4bd 164 }
aberk 0:976ab2e4e4bd 165
aberk 0:976ab2e4e4bd 166 }
aberk 0:976ab2e4e4bd 167
aberk 0:976ab2e4e4bd 168 void IMUfilter::computeEuler(void){
aberk 0:976ab2e4e4bd 169
aberk 0:976ab2e4e4bd 170 //Quaternion describing orientation of sensor relative to earth.
aberk 0:976ab2e4e4bd 171 double ESq_1, ESq_2, ESq_3, ESq_4;
aberk 0:976ab2e4e4bd 172 //Quaternion describing orientation of sensor relative to auxiliary frame.
aberk 0:976ab2e4e4bd 173 double ASq_1, ASq_2, ASq_3, ASq_4;
aberk 0:976ab2e4e4bd 174
aberk 0:976ab2e4e4bd 175 //Compute the quaternion conjugate.
aberk 0:976ab2e4e4bd 176 ESq_1 = SEq_1;
aberk 0:976ab2e4e4bd 177 ESq_2 = -SEq_2;
aberk 0:976ab2e4e4bd 178 ESq_3 = -SEq_3;
aberk 0:976ab2e4e4bd 179 ESq_4 = -SEq_4;
aberk 0:976ab2e4e4bd 180
aberk 0:976ab2e4e4bd 181 //Compute the quaternion product.
aberk 0:976ab2e4e4bd 182 ASq_1 = ESq_1 * AEq_1 - ESq_2 * AEq_2 - ESq_3 * AEq_3 - ESq_4 * AEq_4;
aberk 0:976ab2e4e4bd 183 ASq_2 = ESq_1 * AEq_2 + ESq_2 * AEq_1 + ESq_3 * AEq_4 - ESq_4 * AEq_3;
aberk 0:976ab2e4e4bd 184 ASq_3 = ESq_1 * AEq_3 - ESq_2 * AEq_4 + ESq_3 * AEq_1 + ESq_4 * AEq_2;
aberk 0:976ab2e4e4bd 185 ASq_4 = ESq_1 * AEq_4 + ESq_2 * AEq_3 - ESq_3 * AEq_2 + ESq_4 * AEq_1;
aberk 0:976ab2e4e4bd 186
aberk 0:976ab2e4e4bd 187 //Compute the Euler angles from the quaternion.
aberk 0:976ab2e4e4bd 188 phi = atan2(2 * ASq_3 * ASq_4 - 2 * ASq_1 * ASq_2, 2 * ASq_1 * ASq_1 + 2 * ASq_4 * ASq_4 - 1);
aberk 0:976ab2e4e4bd 189 theta = asin(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_3);
aberk 0:976ab2e4e4bd 190 psi = atan2(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_4, 2 * ASq_1 * ASq_1 + 2 * ASq_2 * ASq_2 - 1);
aberk 0:976ab2e4e4bd 191
aberk 0:976ab2e4e4bd 192 }
aberk 0:976ab2e4e4bd 193
aberk 0:976ab2e4e4bd 194 double IMUfilter::getRoll(void){
aberk 0:976ab2e4e4bd 195
aberk 0:976ab2e4e4bd 196 return phi;
aberk 0:976ab2e4e4bd 197
aberk 0:976ab2e4e4bd 198 }
aberk 0:976ab2e4e4bd 199
aberk 0:976ab2e4e4bd 200 double IMUfilter::getPitch(void){
aberk 0:976ab2e4e4bd 201
aberk 0:976ab2e4e4bd 202 return theta;
aberk 0:976ab2e4e4bd 203
aberk 0:976ab2e4e4bd 204 }
aberk 0:976ab2e4e4bd 205
aberk 0:976ab2e4e4bd 206 double IMUfilter::getYaw(void){
aberk 0:976ab2e4e4bd 207
aberk 0:976ab2e4e4bd 208 return psi;
aberk 0:976ab2e4e4bd 209
aberk 0:976ab2e4e4bd 210 }
aberk 1:8a920397b510 211
aberk 1:8a920397b510 212 void IMUfilter::reset(void) {
aberk 1:8a920397b510 213
aberk 1:8a920397b510 214 firstUpdate = 0;
aberk 1:8a920397b510 215
aberk 1:8a920397b510 216 //Quaternion orientation of earth frame relative to auxiliary frame.
aberk 1:8a920397b510 217 AEq_1 = 1;
aberk 1:8a920397b510 218 AEq_2 = 0;
aberk 1:8a920397b510 219 AEq_3 = 0;
aberk 1:8a920397b510 220 AEq_4 = 0;
aberk 1:8a920397b510 221
aberk 1:8a920397b510 222 //Estimated orientation quaternion elements with initial conditions.
aberk 1:8a920397b510 223 SEq_1 = 1;
aberk 1:8a920397b510 224 SEq_2 = 0;
aberk 1:8a920397b510 225 SEq_3 = 0;
aberk 1:8a920397b510 226 SEq_4 = 0;
aberk 1:8a920397b510 227
aberk 1:8a920397b510 228 }