I incorporated a Kalman filter into the altitude control loop; it estimates altitude and velocity from a model of the helicopter and the altitude measurement from the LIDAR Lite. It doesn’t compensate for tilt errors, so it only works for small pitch and roll angles. The dynamics model includes a lag between issuing the thrust command and when the rotor thrust reaches steady state, so a transfer function Tactual/Tcommand = 1/(tau*s + 1) goes between the controller transfer function and the plant. This plot shows a climb to 20 m then a descent at 1 m/s. The black curve is the Kalman filter altitude estimate and the red points are the noisy LIDAR measurements.

This plot shows the commanded control output and the actual control output for tau = 0.9 seconds.