function_xy_to_sita.m

卡尔曼滤波估计雅可比的matlab代码,用于计算机视觉的在线估计

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;