Here is the code for the filter that calculates attitude from gyro and accelerometer. The functional bits of the code are well commented in the code itself. The main structure of the code will be described in this post. A practical description of the filter can be found in this article. The theory is in this bunch of articles.
The filter loops, giving frequent updates of an estimation of the robots attitude. Therefore the code is in one endless loop. The faster the filter loops the better. But the execution speed is limited due to hardware limitation and other processes that need CPU time. To preserve time for other processes the iteration speed of the program is controlled. At the start of the program the sensors and data structures need to be initialized. Hardware initialization is done in a separate function called init (not included in the code below). This concentrates all hardware depended code into one function and keeps the main function easily reusable, even if you have different hardware.
As stated before the loop contains the filter. Aside from the code that implements the filter there is code to supply information to the user. These parts of the code are all commented away. If they are useful to you, you can uncomment them. Even then you can switch these parts of the code on or of by changing a true to false in the if statement surrounding this code. There are three ways to supply information to the end user. Data can be logged to excel, displayed on the NXT screen or a full color led (BLINKM) can be triggered if certain conditions are met.
If you find the code useful or informative please press the “like” button. This might encourage me to make more posts like this.
#pragma systemfile
#pragma autostarttasks;
// this defines the iteration time of the filter in msec
#define IMU_speed 25
#include "math.c"
#include "matrixDirectives.c"
#include "I2C.c"
#include "IMU_acclMatrix.c"
#include "IMU_gyroMatrix.c"
#include "IMU_blinkM.c"
/*
// Optional code to log the filter state in an excel fie
#define NXT2excelMaxData 8
#include "nxt2excel.c"
*/
// this struct will hold attitude, position (Todo) and speed (Todo) of the robot
typedef struct
{
matrix C;
bool initialized;
} tPosition;
tPosition position;
void init();
void normalizeM(matrix &in);
task main()
{
float duration;
matrix U, T;
vector I, P, Rate;
long lastTime, start, wait;
bool home, oldhome;
// PI controller parameters, these are used to tune the filter
float kp=0.0005,ki=0.005, id=0.99;
// Initialize the sensors
init();
lastTime=nPgmTime;
eraseDisplay();
// initialize cosine matrix, the cosine matrix holds the attitude of the robot
position.C[0][0]=1;
position.C[0][1]=0;
position.C[0][2]=0;
position.C[1][0]=0;
position.C[1][1]=1;
position.C[1][2]=0;
position.C[2][0]=0;
position.C[2][1]=0;
position.C[2][2]=1;
// iterate forever, but halt if one of the sensorsreturns an error
while (I2C_gyro.error==NO_ERR && I2C_accl.error==NO_ERR)
{
start=nPgmTime;
alive();
// get sensor readings,
// gyro readings (in rad/sec) are stored in a vector (array of 3 elements) called gyro.rate
// acceleration readings (in g) are stored in a vector called accl.g
Gyro_getGyroBiased();
Accl_getAcclBiased();
// for better filter output the exact time for each iteration is measured
duration=(nPgmTime-lastTime)/1000.0;
lastTime=nPgmTime;
// nPgmTime wraps around when reaching maxint, correct for this
if (duration<0) duration=IMU_speed/1000.0;
/*
// Optional code to log the filter state in an excel fie
if (false)
{
for (int Rcol=0;Rcol<3;Rcol++)
{
NXT2excelAddValue(1,gyro.rate[Rcol]*1000);
NXT2excelAddValue(2,accl.g[Rcol]*1000);
NXT2excelAddValue(3,duration*1000.0);
NXT2excelAddValue(4,I[Rcol]*1000);
NXT2excelAddValue(5,P[Rcol]*1000);
for (int ii=0;ii<3;ii++)
NXT2excelAddValue(6+ii,position.C[Rcol][ii]*1000.0);
NXT2excelSendMessage();
}
}
*/
// scale the turning rate (rad/sec) to a change in attitude (rad)
VscaleVS(gyro.rate,duration,Rate);
// corract the gyro signal for offset (I) and accumulated errors (P), but do it slowly (ki, kp)
// at the same time, convert it to a skew symmetric matrix
// for speed not matrix functions are used here
U[2][1]=gyro.rate[0]*duration-I[0]*ki-P[0]*kp;
U[1][2]=-U[2][1];
U[0][2]=gyro.rate[1]*duration-I[1]*ki-P[1]*kp;
U[2][0]=-U[0][2];
U[1][0]=gyro.rate[2]*duration-I[2]*ki-P[2]*kp;
U[0][1]=-U[1][0];
// transform the change in attitude in respect to robot axis into change in attitude in respect to global axis
// for speed not matrix functions are used here
// matrix equivalent: MproductMM(position.C,U,T);
T[0][0]=position.C[0][1]*U[1][0]+position.C[0][2]*U[2][0];
T[0][1]=position.C[0][0]*U[0][1]+position.C[0][2]*U[2][1];
T[0][2]=position.C[0][0]*U[0][2]+position.C[0][1]*U[1][2];
T[1][0]=position.C[1][1]*U[1][0]+position.C[1][2]*U[2][0];
T[1][1]=position.C[1][0]*U[0][1]+position.C[1][2]*U[2][1];
T[1][2]=position.C[1][0]*U[0][2]+position.C[1][1]*U[1][2];
T[2][0]=position.C[2][1]*U[1][0]+position.C[2][2]*U[2][0];
T[2][1]=position.C[2][0]*U[0][1]+position.C[2][2]*U[2][1];
T[2][2]=position.C[2][0]*U[0][2]+position.C[2][1]*U[1][2];
// add the cange in attitude to the attitude to get an updated attitude
MsumMM(position.C,T,position.C);
// Renormalize matrix
// due to small errors the matrix gets corrupted over time (its axis are no longer perpendicular
// this function corrects this
normalizeM(position.C);
// correct the attitude using tilt info from the accelerometer
// but only if there is no or little acceleration, otherwise titl info can not be trusted
if (abs(sqrt(accl.g[0]*accl.g[0]+accl.g[1]*accl.g[1]+accl.g[2]*accl.g[2])-1.0)<.2)
{
// Calculate difference (error) between culculated attitude and accelerometer
// and use this in a proportional feedback (P)
P[0] = (position.C[2][1]*accl.g[2]) - (position.C[2][2]*accl.g[1]);
P[1] = (position.C[2][2]*accl.g[0]) - (position.C[2][0]*accl.g[2]);
P[2] = (position.C[2][0]*accl.g[1]) - (position.C[2][1]*accl.g[0]);
// Add the error to the accumulated error and use this in a integral feedback (I)
I[0]=I[0]*id+P[0]*duration;
I[1]=I[1]*id+P[1]*duration;
I[2]=I[2]*id+P[2]*duration;
}
else
{
P[0]=0;
P[1]=0;
P[2]=0;
}
/*
// optional code to drive the LED in the sensor
if (true)
{
// test if the robot is in it's starting attitude
if ( abs(position.C[2][0])<.009 && abs(position.C[2][1])<.009)
home=true;
else
home=false;
// change the color of the LED if needed
if (home != oldhome)
{
if (home)
blinkM_setColour(0 , 0, 128);
else
blinkM_setColour(64 , 0, 0);
}
oldhome=home;
}
*/
// optional code to display sensor and filter data
if (true && time1[T1]>200)
{
eraseDisplay();
nxtDisplayTextLine(7, "%5d",wait);
//displayM(position.C);
//displayV(accl.g,1);
//displayV(gyro.rate,0);
ClearTimer(T1);
}
//while (nNxtButtonPressed==-1);
//while (nNxtButtonPressed!=-1);
// wait for next iteration
// the program does not assume a constant execution speed,
// instead it measures every iteration, and waits the exact amount of time to guarantue constant iteration speed
wait=IMU_speed-(nPgmTime-start);
if (wait<0)
{
wait=0;
//PlaySound(soundException);
}
wait1Msec(wait);
}
}
void init()
{
// configure port;
SensorType[S3]=sensorI2CCustom;
nI2CRetries=3;
position.initialized=false;
accl.initialized=false;
gyro.initialized=false;
// configure blinkM;
I2C_blinkM.port=S3;
I2C_blinkM.address=0x12;
blinkM_setColour(0 , 4, 0);
// configure accl;
I2C_accl.port=S3;
I2C_accl.address=0xa6;
Accl_init();
Accl_getBias();
accl.initialized=true;
// configure gyro;
I2C_gyro.port=S3;
I2C_gyro.address=0xD0;
Gyro_init();
Gyro_getBias();
gyro.initialized=true;
if (I2C_gyro.error || I2C_accl.error)
{
nxtScrollText("IMU: %3d, %3d",I2C_gyro.error , I2C_accl.error);
blinkM_setColour(63 , 0, 0);
PlaySound(soundException);
wait1Msec(2000);
}
blinkM_setColour(0 , 0 , 0);
position.initialized=true;
}
//normalizes a matrix (takes 2.87 msec)
void normalizeM(matrix &in)
{
float error, renorm;
vector vx,vy, xor, yor, zor;
VgetRowMS(in, 0,vx);
VgetRowMS(in, 1,vy);
dotProductVVS(vx, vy,error); // eq.18
if (abs(error)>0.05)
{
VscaleVS(vy,error/-2.0, xor);
VsumVV(vx, xor, xor); //eq.19
VscaleVS(vx,error/-2.0, yor);
VsumVV(vy, yor, yor); //eq.19
VcrossProductVV(xor,yor, zor); //eq.20
dotProductVVS(xor, xor,error);
renorm = .5 *(3 - error); //eq.21
for ( int Rcol=0; Rcol<Msize;Rcol++)
in[0][Rcol]=renorm*xor[Rcol];
dotProductVVS(yor, yor,error);
renorm = .5 *(3 - error); //eq.21
for ( int Rcol=0; Rcol<Msize;Rcol++)
in[1][Rcol]=renorm*yor[Rcol];
dotProductVVS(zor, zor,error);
renorm = .5 *(3 - error); //eq.21
for ( int Rcol=0; Rcol<Msize;Rcol++)
in[2][Rcol]=renorm*zor[Rcol];
}
}
NXT time 