VZ added

Added gps vertical velocity
Remove these two lines. We don't need to initialize Z axis, estimate is already valid (based on baro).