chan算法时差定位的一种使用算法

clear
x_1=0;y_1=0;x_2=750;y_2=433;x_3=750;y_3=-433;
x21=x_2-x_1;y21=y_2-y_1;x31=x_3-x_1;y31=y_3-y_1;
D1=x_1^2+y_1^2;D2=x_2^2+y_2^2;D3=x_3^2+y_3^2;

Ts=0.5;%采样间隔s
Num=100;%个数
x0=100;y0=100;% 初始位置
vx=10;vy=10;%速度
x（1）=x0+Ts.*vx（1）;
y（1）=y0+Ts.*vy（1）;
r1（1）=sqrt（（x（1）-x_1）^2+（y（1）-y_1）^2）;
r2（1）=sqrt（（x（1）-x_2）^2+（y（1）-y_2）^2）;
r3（1）=sqrt（（x（1）-x_3）^2+（y（1）-y_3）^2）;
r0（1）=sqrt（x（1）.^2+y（1）.^2）;

for i=2:Num
    x（i）=x（i-1）+Ts.*vx;
    y（i）=y（i-1）+Ts.*vy;
    r1（i）=sqrt（（x（i）-x_1）^2+（y（i）-y_1）^2）;
    r2（i）=sqrt（（x（i）-x_2）^2+（y（i）-y_2）^2）;
    r3（i）=sqrt（（x（i）-x_3）^2+（y（i）-y_3）^2）;
    r0（i）=sqrt（x（i）.^2+y（i）.^2）;
end
d_n=50;

noise=randn（1Num）;
r21=r2-r1+d_n.*noise;
r31=r3-r1+d_n.*noise;
r01=r0+r1+d_n.*noise;


qq=d_n^2.*[1 0.5 0.5;0.5 1 0.5;0.5 0.5 1];

for k=1:Num
g_a1=-[x21 y21 r21（k）;x31 y31 r31（k）;-x_1 -y_1 -r01（k）];
h_a1=0.5*[r21（k）.^2-D2+D1;r31（k）.^2-D3+D1;r01（k）.^2+D1];
the_10=inv（g_a1‘*inv（qq）*g_a1）*g_a1‘