IMU noise params were in the wrong order for G. Filter still diverges, but it's getting better.