Revision history [back]

Wow, there's a lot going on here. If I were you, I would start small, and work my way up. I'd get those wheel encoders involved again, and start with an extremely simple EKF setup that just fuses the wheel encoder data with the IMU data. Make sure that is behaving and doing what you expect, then add a new sensor. Make sure you know the coordinate frames of all your sensor data and have your transforms set up accordingly.