IMUfilter -

Users » SED9008 » Code » IMUfilter

IMUfilter.cpp@0:4e7171d6f97e, 2012-06-01 (annotated)

Committer:: SED9008
Date:: Fri Jun 01 08:16:53 2012 +0000
Revision:: 0:4e7171d6f97e

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	19 Apr 2012
Imports:	5
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	0
Followers:	1

This repository is Public (Unlisted).

IMUfilter.cpp@0:4e7171d6f97e, 2012-06-01 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning