Added GPS time to highres_imu message.
Fixed altitude initialisation.
Testing higher order filter on vario.