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‘