function F=func1(param,dt,dx,dy,dtheta,l)
%param(1) =  theta
%param(2) =  phi
%param(3) =  v

f1=cos(param(1))*cos(param(2))*param(3)*dt-dx;
f2=sin(param(1))*cos(param(2))*param(3)*dt-dy;
f3=sin(param(2))*param(3)*dt/l-dtheta;


F=abs(f1) + abs(f2) + 10*abs(f3); %*f1+f2*f2+f3*f3;