A double stage Kalman filter for sensor fusion and orientation tracking in 9D IMU

This work presents an orientation tracking system based on a double stage Kalman filter for sensor fusion in 9D IMU. The IMU is composed by a 3D gyro, a 3D accelerometer and a magnetic compass. The filter was divided into two stages to reduce algorithm complexity. Gyro data are used to first estimate the angular position, then the first stage corrects roll and pitch angles using accelerometer data. The second stage processes magnetic compass data to correct the yaw angle. One of the advantages of this kind of filter is that a magnetic anomaly does not influence roll and pitch estimation accuracy. The flexibility is also desirable, because if the magnetic compass is not available, it is simply possible to switch off the second stage of the filter. In this work an ASIP was designed to process the filter algorithm and a proof of concept on FPGA was successfully realized. In the future the ASIP will be integrated within the logic of a new 6D sensor that could be optionally interfaced with an external magnetic compass.