I have included a compass into my INS filter. It now is drift-free and accurate over all three axis. It also is very robust to magnetic disturbances. The compass itself is not. It gives me great pleasure to see the first results.

Tuning the filter is difficult though. I have used the NXJChartingLogger that will be part of Lejos 0.9.1 a lot. The filter provides all kinds of output that is displayed real time on the pc. Different parameters are logically grouped into different graphs. The graphs provide information on the state of the filter and thus help in tuning.
I have also included a method to introduce errors in the sensor data. This enables me to examine the performance of the filter.

As a last thing I have made the filter calculate the initial state of the filter. With this the filter settles accurately (<.5degrees) within half a second.

The performance dropped to 97 Hertz with three sensors attached. But I haven't optimized the compass code yet.

In later posts I'll demonstrate the performance of the filter.