z_drive: fixed error in minCost
z_drive: set default gains
z_drive: updated code