Working Code before intertial measurements