IMU + Encoder - sinnvolle Filter

Hallo,

mein Roboter hat inzwischen eine relativ gute Positionsbestimmung, danke encoder und dead reckoning.
Wie Rotation/Yaw weicht aber relativ schnell ab. Damit dann auch die X/Y Werte.

Daher möchte ich jetzt noch meine IMU mit einrechnen. Die Kombination von Gyro und Accelerometer funktioniert recht gut. Die Drift ist auch minimal. (DCM Algorithmus (den habe ich geklaut und nur eine Kalibrierung hinzugefügt)
Die Frage ist jetzt, wie ich am besten die Yaw Werte mit den Winkelberechnungen der Odometry kombinieren.

Meine Einfachen Ansätze (Erfolgsmessungen stehen noch aus):
1. (theta2) Update anhand der IMU Daten nur, wenn auch die Odometry eine Drehung feststellen würde. (minimiert die Auswirkung der Drift, minimiert aber nicht die Auswirkung der Encoderauflösung)
2. (theta3) Update anhand der IMU, sobald die Odometry auch eine Bewegung feststellt, egal ob geradeaus oder nicht. (minimiert die Auswirkung der Drift weniger als Option 1, minimiert aber die Auswirkung der Encoderauflösung - da die Drehwinkel nur von der IMU kommen, auch bei der Geradeausfahrt)

Code:

---

if (left_ticks != right_ticks){ theta2+=yaw-yawLast;}
if (left_ticks != 0 || right_ticks != 0) {theta3+=yaw-yawLast;}

yawLast=yaw;

---

Gibt es noch sinnvollere Ansätze, bei denen Eventuell auch die Drehwinkel der Encoder eingerechnet werden? Ich könnte natürlich beide mitteln. Dann würde ich aber zum Beispiel bei Stillstand des Roboters die Drift nicht wegbekommen.

Gruß
Robert