tdoa3_bijiao.asv

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(1,Num);
r21=r2-r1+d_n.*noise;
noise=randn(1,Num);
r31=r3-r1+d_n.*noise;
noise=randn(1,Num);
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];
theta_1_temp=inv(g_a1'*inv(qq)*g_a1)*g_a1'*inv(qq)*h_a1;  

b_1_temp=zeros(3,3);
b_1_temp(1,1)=sqrt((x_2-theta_1_temp(1,1))^2+(y_2-theta_1_temp(2,1))^2);
b_1_temp(2,2)=sqrt((x_3-theta_1_temp(1,1))^2+(y_3-theta_1_temp(2,1))^2);
b_1_temp(3,3)=theta_1_temp(3,1);
ff1=4.*b_1_temp*qq*b_1_temp;
theta_1=inv(g_a1'*inv(ff1)*g_a1)*g_a1'*inv(ff1)*h_a1;   
%第二步
cov_theta1=inv(g_a1'*inv(ff1)*g_a1);
b_2=zeros(3,3);
b_2(1,1)=theta_1(1,1)-x_1;
b_2(2,2)=theta_1(2,1)-y_1;
b_2(3,3)=theta_1(3,1);
dafy2=4.*b_2*cov_theta1*b_2;

g_a2=[1 0;0 1;1 1];
h_a2=[(theta_1(1,1)-x_1)^2;(theta_1(2,1)-y_1)^2;(theta_1(3,1))^2];

theta_2=inv(g_a2'*inv(dafy2)*g_a2)*g_a2'*inv(dafy2)*h_a2;

theta(1,k)=sqrt(theta_2(1,1))+x_1;% x
theta(2,k)=sqrt(theta_2(2,1))+y_1;% y
end

figure
plot(x,y)
hold on
plot(theta(1,:),theta(2,:),'r');grid on;
plot(theta(1,:),theta(2,:),'*r');

err=sqrt((theta(1,:)-x).^2+(theta(2,:)-y).^2);
merr=mean(err([2:Num]))
figure
plot(err,'o');
hold on
plot(err);
% title('误差') 

nmse0=((theta(1,:)-x).^2+(theta(2,:)-y).^2)./(x.^2+y.^2);
nmse=mean(nmse0)