How to take 2 sets of coordinates and altitude and calculate angle for Servos

They aren't?

No, they are controlled using the Servo library. How many of your servos are connected to PWM pins? How many NEED to be?