Added temporary code for sliding windows for measurements and updating position for the horizontal IMU
Added temporary code for sliding windows for measurements and updating position for the horizontal IMU