...
...
    x = xest(1,k)+zlaser(1)*cos(xest(3,k)-pi/2+zlaser(2));
    y = xest(2,k)+zlaser(1)*sin(xest(3,k)-pi/2+zlaser(2));
...
...