running program with video and navdata
started iplementing height control
started adding height controller - PID