g = 10;                         
h = .1;
K = 100;
m = 1;
M=4;
alpha=0.1;
pmax = 5;
% way-points
k1=30; w1=[-2; 2];
k2=50; w2=[ 3; 3];
k3=70; w3=[-4; -1];
k4=100; w4=[2; -4];
% angles of the thrusters
theta1 = 50*pi/180;
theta2 = 120 *pi/180;