Nonlinear z controller done. Need to do x and y. Added an integral controller to z axis for removing steady state error.