Currently, the system dynamics in IMUFilter do not properly account for the fact that the IMU measurements are in Robot Frame, and the output if it stays as an Odometry measurement needs to be in the World frame. We need to add either a transform when the input/measurement are extracted from the sensor itself from robot to world (likely just a simple rotation matrix on the rx/ry/rz and ax/ay/az vectors), or adjust the actual system dynamics to be non-linear and account for the transform.
Currently, the system dynamics in IMUFilter do not properly account for the fact that the IMU measurements are in Robot Frame, and the output if it stays as an Odometry measurement needs to be in the World frame. We need to add either a transform when the input/measurement are extracted from the sensor itself from robot to world (likely just a simple rotation matrix on the rx/ry/rz and ax/ay/az vectors), or adjust the actual system dynamics to be non-linear and account for the transform.