Add 3d view, adjust IMU algorithm