In my previous post I discussed the Kalman filter for fusing compass and gyro readings. This time I’ll discuss the way I implement the filter on the NXT. I use robotC as my programming environment but most things discussed in this post are relevant to all environments.

The formulas of the Kalman filter use matrices from linear math. They provide a convenient way to write down the various elements of the filter. Robotc on the other hand has no native support for matrix language. Most computer languages don’t support matrices.
This leads me to the first decision I’ll have to make. I have two alternatives. The first is to translate my filter into ordinary functions, the second is to write a function library to support matrix calculus.
The first option has the advantage of speed because it allows me to eliminate arguments and apply other optimizations. The disadvantage of this solution is that my functions can get complex and don’t resemble the model. This will make debugging difficult. But more important it will make it hard to change the model If I need to. It is mainly for this reason I don’t go for this option.
The other option is to implement support for matrix functions. There are only three matrix functions that I need: sum, product and inversion. That cannot be to hard, or can it? Well, matrices can have any number of rows and columns, a good function library should support this. This kind of flexibility is hard to get in robotC. My model only uses scalars (2×1 matrices) and 2×2 matrices. I will make a function library that works for these dimensions only.

The second step is to define the data types to store matrices and scalars. Here again, I sacrifice on flexibility and limit myself to 2×1 scalars and 2×2 matrices. All elements must be of type float because the filter uses real numbers, not integers. Here are the type definitions:

// Matrix size and scalar size;
const int Msize=2;

typedef float matrix[Msize][Msize];
typedef float scalar[Msize];

The third step is to decide how to pass information to my function library and back. This decision is language specific. Robotc supports “pass by reference” this means that one can tell a function were its input is located instead of passing al the input values. It is like me pointing out an article on Wikipedia instead of explaining things myself to you. This technique saves variable space and the amount of parameters used with functions. Therefore I will use this technique.
The result of each function in the library is a scalar or matrix. RobotC does not support user defined data types as function types. In other words, I cannot write functions that return a matrix. But I can pass a reference to a matrix that will be used to update. Therefore I’ll provide my functions a storage location as one of the input parameters. This design choice give the following general syntax for functions in my function library:

Void matrixFunction(inputMatrix1 <,inputMatrix2...inputMatrixN> , resultMatrix);

I decided that my functions won’t be flexible enough to handle matrices of different dimensions. However, My filter requires me to calculate the product of 2*1 and 2*2 matrices and the product of 2*2 and 2*2 matrices. As a consequence of my design decision I’ll have to write two product function, one to handle the first case and one to handle the second case.

The last thing to mention are the naming conventions I use in my function library. Function names start with a character indicating the return type of the function, M for functions that return a 2*2 matrix, S for functions that return a scalar. (technically all functions return void, What I mean is the functional return type). The return type indicator is followed by the name of the mathematical operation, sum, product, transpose or inverse. Function names are completed with characters indicating the type of the input parameters. The function SproductMS calculates the product of a matrix and a scalar and returns a scalar.

Having explained all the design disicions I needed to make, it is time to show the code for the function library.

Warning: This code is not (always) working properly in version 2.01 due to a robotC bug using structs and floats.

// returns the product of 2 matrices in matrix r
void MproductMM(matrix &m1, matrix &m2, matrix &r)
{
int Rrow, Rcol, i;
for (Rrow=0;Rrow
{
for (Rcol=0;Rcol
{
r[Rrow][Rcol]=0;
for (i=0;i
r[Rrow][Rcol]+=m1[Rrow][i]*m2[i][Rcol];
}
}
}

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

// returns the difference of 2 matrices in matrix r
void MsubstractMM(matrix &m1, matrix &m2, matrix &r)
{
int Rrow, Rcol;
for (Rrow=0;Rrow
{
for (Rcol=0;Rcol
r[Rrow][Rcol]=m1[Rrow][Rcol]-m2[Rrow][Rcol];
}
}

// Inverts a matrix
void MinvertM(matrix &m, matrix &r)
{
float d;
d=m[0][0]*m[1][1]-m[0][1]*m[1][0];  // determinant

if (d==0)
PlaySound(soundException);
else
{
r[0][0]= m[1][1]/d;
r[0][1]=-m[0][1]/d;
r[1][0]=-m[1][0]/d;
r[1][1]= m[0][0]/d;
}
}

// Transposes a matrix
void MtransposeM(matrix &m, matrix &r)
{
int Rrow, Rcol;
for (Rrow=0;Rrow
{
for (Rcol=0;Rcol
r[Rcol][Rrow]=m[Rrow][Rcol];
}
}

// copies one matrix into another
void McopyM(matrix &m, matrix &r)
{
int Rrow, Rcol;
for (Rrow=0;Rrow
{
for (Rcol=0;Rcol<Msize;Rcol++)
r[Rrow][Rcol]=m[Rrow][Rcol];
}
}
// sums two scalars
void SsumSS(scalar &s1, scalar &s2, scalar &r)
{
int Rrow;
for (Rrow=0;Rrow
r[Rrow]=s1[Rrow]+s2[Rrow];
}

//substracts two scalars
void SsubstractSS(scalar &s1, scalar &s2, scalar &r)
{
int Rrow;
for (Rrow=0;Rrow<Msize;Rrow++)
r[Rrow]=s1[Rrow]-s2[Rrow];
}

// copies one scalar into another
void ScopyS(scalar &s, scalar &r)
{
int Rrow;
for (Rrow=0;Rrow<Msize;Rrow++)
r[Rrow]=s[Rrow];
}

// calculates the product of a matrix and a scalar
void SproductMS(matrix &m, scalar &s, scalar &r)
{
int Rrow,  Rcol;
for (Rrow=0;Rrow<Msize;Rrow++)
{
r[Rrow]=0;
for (Rcol=0;Rcol<Msize;Rcol++)
r[Rrow]+=m[Rrow][Rcol]*s[Rcol];
}
}