How to Implements Quaternion in Attitude and heading reference system (AHRS)?

Projects Programming
1

Hi, i want to make Attitude and heading reference system (AHRS) using quaternion algorithm. And I found this code on the internet:

This is header file code (AHRS.h):

//=====================================================================================================
// AHRS.h
// S.O.H. Madgwick
// 25th August 2010
//=====================================================================================================
//
// See AHRS.c file for description.
// 
//=====================================================================================================
#ifndef AHRS_h
#define AHRS_h

//----------------------------------------------------------------------------------------------------
// Variable declaration

extern float q0, q1, q2, q3;	// quaternion elements representing the estimated orientation

//---------------------------------------------------------------------------------------------------
// Function declaration

void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);

#endif
//=====================================================================================================
// End of file
//=====================================================================================================

and this is the main code:

//=====================================================================================================
// AHRS.c
// S.O.H. Madgwick
// 25th August 2010
//=====================================================================================================
// Description:
//
// Quaternion implementation of the 'DCM filter' [Mayhony et al].  Incorporates the magnetic distortion
// compensation algorithms from my filter [Madgwick] which eliminates the need for a reference
// direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
// axis only.
//
// User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
//
// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
// orientation.  See my report for an overview of the use of quaternions in this application.
//
// User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'),
// accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data.  Gyroscope units are
// radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised.
//
//=====================================================================================================

//----------------------------------------------------------------------------------------------------
// Header files

#include "AHRS.h"
#include <math.h>

//----------------------------------------------------------------------------------------------------
// Definitions

#define Kp 2.0f			// proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.005f		// integral gain governs rate of convergence of gyroscope biases
#define halfT 0.5f		// half the sample period

//---------------------------------------------------------------------------------------------------
// Variable definitions

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;	// quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0;	// scaled integral error

//====================================================================================================
// Function
//====================================================================================================

void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
	float norm;
	float hx, hy, hz, bx, bz;
	float vx, vy, vz, wx, wy, wz;
	float ex, ey, ez;

	// auxiliary variables to reduce number of repeated operations
	float q0q0 = q0*q0;
	float q0q1 = q0*q1;
	float q0q2 = q0*q2;
	float q0q3 = q0*q3;
	float q1q1 = q1*q1;
	float q1q2 = q1*q2;
	float q1q3 = q1*q3;
	float q2q2 = q2*q2;   
	float q2q3 = q2*q3;
	float q3q3 = q3*q3;          
	
	// normalise the measurements
	norm = sqrt(ax*ax + ay*ay + az*az);       
	ax = ax / norm;
	ay = ay / norm;
	az = az / norm;
	norm = sqrt(mx*mx + my*my + mz*mz);          
	mx = mx / norm;
	my = my / norm;
	mz = mz / norm;         
	
	// compute reference direction of flux
	hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
	hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
	hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);         
	bx = sqrt((hx*hx) + (hy*hy));
	bz = hz;        
	
	// estimated direction of gravity and flux (v and w)
	vx = 2*(q1q3 - q0q2);
	vy = 2*(q0q1 + q2q3);
	vz = q0q0 - q1q1 - q2q2 + q3q3;
	wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
	wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
	wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  
	
	// error is sum of cross product between reference direction of fields and direction measured by sensors
	ex = (ay*vz - az*vy) + (my*wz - mz*wy);
	ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
	ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
	
	// integral error scaled integral gain
	exInt = exInt + ex*Ki;
	eyInt = eyInt + ey*Ki;
	ezInt = ezInt + ez*Ki;
	
	// adjusted gyroscope measurements
	gx = gx + Kp*ex + exInt;
	gy = gy + Kp*ey + eyInt;
	gz = gz + Kp*ez + ezInt;
	
	// integrate quaternion rate and normalise
	q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
	q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
	q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
	q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
	
	// normalise quaternion
	norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
	q0 = q0 / norm;
	q1 = q1 / norm;
	q2 = q2 / norm;
	q3 = q3 / norm;
}

//====================================================================================================
// END OF CODE
//====================================================================================================

But, i really confusing to implements that algorith. Am i must change all variables with some value? I know for variables gx, gy, gz, ax, ay, az, mx, my, and mz i must change the value with accelerometer, Gyroscope and magnetometer value. But how with other variables like q0, q1, q2, q2, halfT, and others? What value that i must put?

Or, there is another way?

Thank You..

2

I don't know much about AHRS, but I do roughly know what a quaternion is. I did study computer graphics and it was part of the math involved in 3D rotations. It's an alternative representation of orientation to Euler angle parametrization. Quaternions can be thought of as an extension to complex numbers. I think in this case you could represent the quaternion as q = q0 + q1 * i + q2 * j + q3 * k, but that would take me half a day to verify since I've forgotten all the math and I'm not sure it's that relevant. You can convert from one representation to another, like to roll, pitch and yaw angles if needed, but it's still safer to do rotation related math in quaternion space than in Euler space.

Anyways, it looks like you are supposed to read 3-coordinate data from a gyro, accelerometer and magnetometer, with whatever libraries you have, and feed that into the function. The function will spit out the quaternion elements into the global variables q0-q3, normalized. However, I would assume that you would need to initialize the components one way or another. Like, figuring out the initial attitude based on gravity element of the accelerometer, right?

Wherever you found that code at, does the same place not include any examples of a main function or anything that calls it? If not, find another implementation that does, and save time in figuring out how it's supposed to work.

3

I think I may need to do this as well.

What are the 'units' the supplied parameters represent from the IMU?

Once its been done, how can it be used?

Examples would be really nice

Cheers A

4

Unit quaternion represents a rotation in 3D space. You can define the rotation like this for example: q=[v*sin(theta/2), cos(theta/2)], where v=3d unit vector you are rotating about and theta=the angle you rotate about the axis. q0-q3 are just the scalar components of that quaternion.

I had a better look at the function and it seems that you just initialize q0-q3 with the initial known orientation of the device and it gives you the new estimated orientation in those same variables after the given time interval (1 second in that case) for the given gyroscope/accelerometer/magnetometer data upon the initial conditions. For the initial orientation you can either use the definition I gave or for example convert Euler angles to a quaternion. For the units the code comments tells you that "Gyroscope units are radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised".