function_xy_to_sita.m

来自「粒子滤波程序」· M 代码 · 共 7 行

function [sita1,sita2]=Function_xy_to_sita(L1,L2,x,y)
s1=acos((x*x+y*y+L1*L1-L2*L2)/(2*sqrt(x*x+y*y)*L1));
s2=acos((x*x+y*y+L2*L2-L1*L1)/(2*sqrt(x*x+y*y)*L2));
sita=atan(y/x);
sita1=sita-s1;
sita2=s1+s2;