Dein Plot Programm vieleicht? Schieb die Daten auf ne CD-Karte und schau sie dir hinterher an.
Scheind ein dein Proz ist am Ende mit der Rechenleistung zu sein schalte aus was du kannst um das zu Testen.
Der Kalman filter braucht ganz schön Leistung wenn er vernünftige Werte bringen soll.

Beim mir werkelt ein P8X32A Prozessor rum (8RISC und 32Bit)