function [zhat, dh] = beacon(x,X)

d = sqrt((x(1)-X(1))^2 + (x(2)-X(2))^2)

zhat = [ d ; -atan2(X(2)-x(2),X(1)-x(1))-x(3) ]
      
dh = [-(x(1)-X(1))/d     -(x(2)-X(2))/d     0
      (x(2)-X(2))/(d^2) (x(1)-X(1))/(d^2) -1 ]