dingwei.m

多基时差雷达定位算法。以雷达测量误差分布规律入手

function f=dingwei(X)
x=1;y=1;z=1;
z1=0;
z2=0;z3=0;
x1=X(1);
x2=X(2);
x3=X(3);
y1=X(4);
y2=X(5);
y3=X(6);
r1=sqrt((x-x1)^2+(y-y1)^2+(y-y1)^2);
r2=sqrt((x-x2)^2+(y-y2)^2+(y-y2)^2);
r3=sqrt((x-x3)^2+(y-y3)^2+(y-y3)^2);
mubiao1=[r1,x,y,z,-r2,-x,x2,-y,z];fenmu1=x2;
mubiao2=[x2*r1,x2*x,x2*y,x2*z,-x3*r1,-x3*x,-x3*y,-x3*z,x3*r2,x3*x,-x3*x2,x3*y,x3*z,-x2*r3,-x2*x,x2*x3,-x2*y,x2*y3,-x2*z];
fenmu2=x2*y3;
mubiao3=[x^2*y3,x2*y^2,-y^2*x3,-x^2*y3,y^2*x3,-x2*y^2,-y*x3*r1,-y*x3*x,-y*x3*z,-x*y3*r2,+x*y3*x2,-x*y3*y,-x*y3*z,+y*x3*r2,+y*x3*x,-y*x3*x2,+y*x3*z,-x2*y*r3,-x2*y*x,+x2*y*x3,+x2*y*y3,-x2*y*z,+x*y3*r1,+x*y3*y,+x*y3*z,+x2*y*r1,+x2*y*x,+x2*y*z,-x2*y3*r1,-x2*y3*x,-x2*y3*y,-x2*y3*z];
fenmu3=x2*z*y3;
f=sum(mubiao1.*mubiao1/x2^2)+sum(mubiao2.*mubiao2/fenmu2^2)+sum(mubiao3.*mubiao3/fenmu3^2);