We set out yesterday to tune the system but it was more difficult than we first had expected. It turned out that the initial poor performance was not a matter of parameter settings but rather of numerical problems.
It showed out that the system worked perfectly (apart from some small bugs which were discovered in the process) when compiled for double precision (64-bits floats) but hade problems when compiled for single precision (32-bits floats). It worked fine with single precision for some old data that we used when initially coding the system. However, when run with the new data, recorded from the current system, the covariance values went bananas. The main difference is that the new data come in at 819[Hz] in comparison with 250[Hz] for the old data. The process noise scale with f^-2, where f is the frequency, while we do not scale the measurement noise with time. This means that the ratio between process noise and measurement noise decreased with roughly a factor 10. For some reason this made the system unhappy running on single precision.
Unfortunately we can only make hardware floating point operations with single precision so the fact that it worked fine with double precision did not help much. We have started the work to pin down the numerical problems but this far still without luck. However, tweeking the parameter values we could still make the system run just fine. Here’s a sample of what the system can do this far
The plot is of me (JO) taking a turn in the corridors of our department building. The plot was displayed in real-time on the laptop used to receive and record the system position estimates.