Working Kalman Filter on the IMU (kinda...)

The KF works for the IMU in the roll and pitch measurements. Yaw needs
integration from the magnetometer readings to work. Changed data types
on some things to make it work.