Added primary math to IMU