IMUfilter -

Users » SED9008 » Code » IMUfilter

IMUfilter.h@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	#ifndef IMU_FILTER_H
SED9008	0:4e7171d6f97e	36	#define IMU_FILTER_H
SED9008	0:4e7171d6f97e	37
SED9008	0:4e7171d6f97e	38	/**
SED9008	0:4e7171d6f97e	39	* Includes
SED9008	0:4e7171d6f97e	40	*/
SED9008	0:4e7171d6f97e	41	#include "mbed.h"
SED9008	0:4e7171d6f97e	42
SED9008	0:4e7171d6f97e	43	/**
SED9008	0:4e7171d6f97e	44	* Defines
SED9008	0:4e7171d6f97e	45	*/
SED9008	0:4e7171d6f97e	46	#define PI 3.1415926536
SED9008	0:4e7171d6f97e	47
SED9008	0:4e7171d6f97e	48	/**
SED9008	0:4e7171d6f97e	49	* IMU orientation filter.
SED9008	0:4e7171d6f97e	50	*/
SED9008	0:4e7171d6f97e	51	class IMUfilter {
SED9008	0:4e7171d6f97e	52
SED9008	0:4e7171d6f97e	53	public:
SED9008	0:4e7171d6f97e	54
SED9008	0:4e7171d6f97e	55	/**
SED9008	0:4e7171d6f97e	56	* Constructor.
SED9008	0:4e7171d6f97e	57	*
SED9008	0:4e7171d6f97e	58	* Initializes filter variables.
SED9008	0:4e7171d6f97e	59	*
SED9008	0:4e7171d6f97e	60	* @param rate The rate at which the filter should be updated.
SED9008	0:4e7171d6f97e	61	* @param gyroscopeMeasurementError The error of the gyroscope in degrees
SED9008	0:4e7171d6f97e	62	* per second. This used to calculate a tuning constant for the filter.
SED9008	0:4e7171d6f97e	63	* Try changing this value if there are jittery readings, or they change
SED9008	0:4e7171d6f97e	64	* too much or too fast when rotating the IMU.
SED9008	0:4e7171d6f97e	65	*/
SED9008	0:4e7171d6f97e	66	IMUfilter(double rate, double gyroscopeMeasurementError);
SED9008	0:4e7171d6f97e	67
SED9008	0:4e7171d6f97e	68	/**
SED9008	0:4e7171d6f97e	69	* Update the filter variables.
SED9008	0:4e7171d6f97e	70	*
SED9008	0:4e7171d6f97e	71	* @param w_x X-axis gyroscope reading in rad/s.
SED9008	0:4e7171d6f97e	72	* @param w_y Y-axis gyroscope reading in rad/s.
SED9008	0:4e7171d6f97e	73	* @param w_z Z-axis gyroscope reading in rad/s.
SED9008	0:4e7171d6f97e	74	* @param a_x X-axis accelerometer reading in m/s/s.
SED9008	0:4e7171d6f97e	75	* @param a_y Y-axis accelerometer reading in m/s/s.
SED9008	0:4e7171d6f97e	76	* @param a_z Z-axis accelerometer reading in m/s/s.
SED9008	0:4e7171d6f97e	77	*/
SED9008	0:4e7171d6f97e	78	void updateFilter(double w_x, double w_y, double w_z,
SED9008	0:4e7171d6f97e	79	double a_x, double a_y, double a_z);
SED9008	0:4e7171d6f97e	80
SED9008	0:4e7171d6f97e	81	/**
SED9008	0:4e7171d6f97e	82	* Compute the Euler angles based on the current filter data.
SED9008	0:4e7171d6f97e	83	*/
SED9008	0:4e7171d6f97e	84	void computeEuler(void);
SED9008	0:4e7171d6f97e	85
SED9008	0:4e7171d6f97e	86	/**
SED9008	0:4e7171d6f97e	87	* Get the current roll.
SED9008	0:4e7171d6f97e	88	*
SED9008	0:4e7171d6f97e	89	* @return The current roll angle in radians.
SED9008	0:4e7171d6f97e	90	*/
SED9008	0:4e7171d6f97e	91	double getRoll(void);
SED9008	0:4e7171d6f97e	92
SED9008	0:4e7171d6f97e	93	/**
SED9008	0:4e7171d6f97e	94	* Get the current pitch.
SED9008	0:4e7171d6f97e	95	*
SED9008	0:4e7171d6f97e	96	* @return The current pitch angle in radians.
SED9008	0:4e7171d6f97e	97	*/
SED9008	0:4e7171d6f97e	98	double getPitch(void);
SED9008	0:4e7171d6f97e	99
SED9008	0:4e7171d6f97e	100	/**
SED9008	0:4e7171d6f97e	101	* Get the current yaw.
SED9008	0:4e7171d6f97e	102	*
SED9008	0:4e7171d6f97e	103	* @return The current yaw angle in radians.
SED9008	0:4e7171d6f97e	104	*/
SED9008	0:4e7171d6f97e	105	double getYaw(void);
SED9008	0:4e7171d6f97e	106
SED9008	0:4e7171d6f97e	107	/**
SED9008	0:4e7171d6f97e	108	* Reset the filter.
SED9008	0:4e7171d6f97e	109	*/
SED9008	0:4e7171d6f97e	110	void reset(void);
SED9008	0:4e7171d6f97e	111
SED9008	0:4e7171d6f97e	112	private:
SED9008	0:4e7171d6f97e	113
SED9008	0:4e7171d6f97e	114	int firstUpdate;
SED9008	0:4e7171d6f97e	115
SED9008	0:4e7171d6f97e	116	//Quaternion orientation of earth frame relative to auxiliary frame.
SED9008	0:4e7171d6f97e	117	double AEq_1;
SED9008	0:4e7171d6f97e	118	double AEq_2;
SED9008	0:4e7171d6f97e	119	double AEq_3;
SED9008	0:4e7171d6f97e	120	double AEq_4;
SED9008	0:4e7171d6f97e	121
SED9008	0:4e7171d6f97e	122	//Estimated orientation quaternion elements with initial conditions.
SED9008	0:4e7171d6f97e	123	double SEq_1;
SED9008	0:4e7171d6f97e	124	double SEq_2;
SED9008	0:4e7171d6f97e	125	double SEq_3;
SED9008	0:4e7171d6f97e	126	double SEq_4;
SED9008	0:4e7171d6f97e	127
SED9008	0:4e7171d6f97e	128	//Sampling period
SED9008	0:4e7171d6f97e	129	double deltat;
SED9008	0:4e7171d6f97e	130
SED9008	0:4e7171d6f97e	131	//Gyroscope measurement error (in degrees per second).
SED9008	0:4e7171d6f97e	132	double gyroMeasError;
SED9008	0:4e7171d6f97e	133
SED9008	0:4e7171d6f97e	134	//Compute beta (filter tuning constant..
SED9008	0:4e7171d6f97e	135	double beta;
SED9008	0:4e7171d6f97e	136
SED9008	0:4e7171d6f97e	137	double phi;
SED9008	0:4e7171d6f97e	138	double theta;
SED9008	0:4e7171d6f97e	139	double psi;
SED9008	0:4e7171d6f97e	140
SED9008	0:4e7171d6f97e	141	};
SED9008	0:4e7171d6f97e	142
SED9008	0:4e7171d6f97e	143	#endif /* IMU_FILTER_H */

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.h@0:4e7171d6f97e, 2012-06-01 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning