for my IMU sensor I created a filter that fuses the readings from the gyro and the accelerometer to get drift free and low noise attitude data. You can read more about this in my previous posts. In this and the next post I give you the code for the filter.

The code consists of three parts. First there is the code to interface with the sensors. This part of the code is very specific to the type of sensors that I used. As you probably have a different gyro and accelerometer it won’t be of much use to you. Therefore I will not publish this part of the code.
The second part of the code consists of function that support matrix and vector math. This part of the code will be presented below. The third part of the code contains the filter itself. This will be the subject of my next post.

The code is written in robotC, but as far as I can see the matrix functions are not robotc specific. If you use another c dialect you should be able to modify it without much hassle. Only the two display functions use robotC specific code, you’ll have to modify this when you use another C dialect.
Some of the choices I made find their origin in using robotC. At first I implemented the library as functions, the vectors and matrices that are the input and result of matrix calculations were passed by reference. However, there seems to be a bug in robotC, once my programs got larger some of the functions behaved odd. So I had to to find another way to implement the calculations. I ended up using directives. Directives are resolved by the precompiler. They have some disadvantages, less readable code, larger compiled programs and slower execution. But they worked without problems. I will call the directives functions for readability, although technically I shouldn’t.

Some words about the conventions I used in the code. Every function is named after the calculation it implements, like sum, substract or dotproduct. In addition to the name, characters are added that supply information about the parameters to the function. Preceeding the name is one character presenting the type of the return parameter, M for matrix, V for vector and S for scalar. Trailing the name are characters representing the input parameters of the function. So VsubstractVV accepts two vectors as input and returns a vector. All parameters, both input and output are supplied as parameters to the function call. There are no real return parameters. To call the VsubstractVV function you write VsubstractVV(inputVector1, inputVector2, outputVector).

#define Msize 3
typedef float matrix[Msize][Msize];
typedef float vector[Msize];


// transforms a vector in a skew symmetric matrix 
#define MskewSymmetricV(v, m ) \
{\
  m[0][0]=0;\
  m[0][1]=-v[2];\
  m[0][2]=v[1];\
  m[1][0]=v[2];\
  m[1][1]=0;\
  m[1][2]=-v[0];\
  m[2][0]=-v[1];\
  m[2][1]=v[0];\
  m[2][2]=0;\
}


// returns the product of 2 matrices in matrix r
#define MproductMM(m1, m2, r)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
  {\
    for ( intRcol=0;Rcol<Msize;Rcol++)\
    {\
      r[Rrow][Rcol]=0;\
      for ( int ii=0;ii<Msize;ii++)\
        r[Rrow][Rcol]=r[Rrow][Rcol]+m1[Rrow][ii]*m2[ii][Rcol];\
    }\
  }\
}

// returns the sum of 2 matrices in matrix r
#define MsumMM(m1, m2, r)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
    for ( int Rcol=0;Rcol<Msize;Rcol++)\
      r[Rrow][Rcol]=m1[Rrow][Rcol]+m2[Rrow][Rcol];\
}


// sums two vectors
#define VsumVV(v1, v2, r)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
    r[Rrow]=v1[Rrow]+v2[Rrow];\
}

// substracts two vectors
#define VsubstractVV(v1, v2, r)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
    r[Rrow]=v1[Rrow]-v2[Rrow];\
}


// calculates the product of a matrix and a vector
#define VproductMV(m, v, r)\
{\
  for ( int Rrow=0;\Rrow<Msize;\Rrow++)\
  {\
    r[Rrow]=0;\
    for ( int Rcol=0;Rcol<Msize;Rcol++)\
      r[Rrow]+=m[Rrow][Rcol]*v[Rcol];\
  }\
}


// returns the dot product of two vectors
#define dotProductVVS(v1, v2, dp)\
{\
    dp=0;\
    for( int ii = 0; ii < Msize; ii++)\
      dp += v1[ii]*v2[ii];\
}

// returns the cross product of two vectors
#define VcrossProductVV(v1,v2, vOut)\
{\
    vOut[0] = (v1[1]*v2[2]) - (v1[2]*v2[1]);\
    vOut[1] = (v1[2]*v2[0]) - (v1[0]*v2[2]);\
    vOut[2] = (v1[0]*v2[1]) - (v1[1]*v2[0]);\
}

// multiplies a vector with a scalar
#define VscaleVS(v,scale, vOut)\
{\
    for( int ii = 0; ii < Msize; ii++)\
      vOut[ii] = v[ii]*(scale);\
}

// returns one row of a matrix in a vector
#define VgetRowMS(m, Rrow, v)\
{\
  for ( int Rcol=0;Rcol<Msize;Rcol++)\
    v[Rcol]=m[Rrow][Rcol];\
}

// returns one column of a matrix in a vector
#define VgetColMS(m, Rcol, v)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
    v[Rrow]=m[Rrow][Rcol];\
}


// Displays the values of a matrix
#define displayM(m)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
    for ( int Rcol=0;Rcol<Msize;Rcol++)\
      nxtDisplayStringAt(1+Rcol*33, 63 - Rrow*9, "%2.2f",m[Rrow][Rcol]);\
}

// Displays the values of a vector
#define displayV(v, start)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
	    nxtDisplayStringAt(1+start*33, 63 - (3+Rrow)*9, "%2.2f",v[Rrow]);\
}

// returns the difference of 2 matrices in matrix r
#define MsubstractMM(m1, m2, r)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
    for ( int Rcol=0;Rcol<Msize;Rcol++)\
      r[Rrow][Rcol]=m1[Rrow][Rcol]-m2[Rrow][Rcol];\
}

// Transposes a matrix
#define MtransposeM(m, r)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
    for ( int Rcol=0;Rcol<Msize;Rcol++)\
      r[Rcol][Rrow]=m[Rrow][Rcol];\
}

// copies one matrix into another
#define McopyM(m, r)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
    for ( int Rcol=0;Rcol<Msize;Rcol++)\
      r[Rrow][Rcol]=m[Rrow][Rcol];\
}



// copies one vector into another
#define VcopyV(v, r)\
{\
  for ( int Rrow=0;Rrow<Msize;Rrow++)\
    r[Rrow]=v[Rrow];\
}

// translate euler angles to direction cosine
#define Meuler2cosineV(v, m)\
{\
  m[0][0]=cos(v[1])*cos(v[2]);\
  m[0][1]=sin(v[0])*sin(v[1])*cos(v[2])-cos(v[0])*sin(v[2]);\
  m[0][2]=cos(v[0])*sin(v[1])*cos(v[2])+sin(v[0])*sin(v[2]);\
  m[1][0]=cos(v[1])*sin(v[2]);\
  m[1][1]=cos(v[0])*cos(v[2])+sin(v[0])*sin(v[1])*sin(v[2]);\
  m[1][2]=cos(v[0])*sin(v[1])*sin(v[2])-sin(v[0])*cos(v[2]);\
  m[2][0]=-sin(v[1]);\
  m[2][1]=sin(v[0])*cos(v[1]);\
  m[2][2]=cos(v[0])*cos(v[1]);\
}

// translate cosine to euler
#define Vcosine2eulerM(m, v)\
{\
  v[0]=atan2(m[2][1],m[2][2]);\
  v[1]=asin(-m[2][0]);\
  v[2]=atan2(m[1][0],m[0][0]);\
}

Advertisements