Aaron Berk / Mbed 2 deprecated RS-EDP-RDS-Rover

Roving robot using the RS-EDP.

Dependencies:   mbed RSEDP_AM_MC1_lib SDFileSystem

IMUfilter.cpp@1:ffef6386027b, 2010-08-26 (annotated)

Committer:
aberk
Date:
Thu Aug 26 14:41:08 2010 +0000
Revision:
1:ffef6386027b
Added additional comments and documentation.

Who changed what in which revision?

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