close all
clc
clear all
Fs=48e3;
Ts=1/Fs;
L=48e3;
c=343;
var_f=10^-10;
var_n=0.5;
var_u=0.05;
count=100;
w1=[1 6 11 16 21 26 31 36 41 46 51 56];
sensor_p=[10 10;10 -10;-10 -10;-10 10;0 10;0 -10;10  0;-10  0];
d=[1,0];
%--------------------------------------------------------
d1=[0.1,0];                                   %第一類源的運動的位移
%--------------------------------------------------------
b=2*pi*rand;
a=[b];
for t=1:54
    a=[a;b+2*pi*t/55];                               %第二類源的運動位移的角度
end
d2=[];
for t=1:55
    d2=[d2;0.1*cos(a(t)) 0.1*sin(a(t))];             %第二類源的運動位移
end
%--------------------------------------------------------
original_source=[0,0];
source1=[original_source;original_source-d];
for n=1:55   
    source1=[source1;original_source-d-(-1)^n.*d1]; %第一類源的運動軌跡
end
source2=[original_source;original_source-d];
for n=1:55
    source2=[source2;original_source-d-d2(n,:)];    %第二類源的運動軌跡
end
    r1=[];
    r2=[];
for t=1:12
    %for n=1:count
    w=w1(t);
    for j=1:(w+1)
        for i=1:8
            r1=[r1 ([source1(j,1) source1(j,2)] -sensor_p(i,:))'/norm([source1(j,1) source1(j,2)]-sensor_p(i,:))];
        end
    end
    for j=1:(w+1)
        for i=1:8
            r2=[r2 ([source2(j,1) source2(j,2)] -sensor_p(i,:))'/norm([source2(j,1) source1(j,2)]-sensor_p(i,:))];
        end
    end
    g1=[];
    g2=[];
    switch w
        case{1}
            for i=1:8
                g11=[(r1(:,i)-r1(:,i+8))/(c*Ts)
                    r1(:,i+8)/(c*Ts)];
                g1=[g1 g11];
                h=[1,-1];
 %--------------------------------------------第二類源
                g22=[(r2(:,i)-r2(:,i+8))/(c*Ts)
                    r2(:,i+8)/(c*Ts)];
                g2=[g2 g22];
                h=[1,-1];
            end
        otherwise
            g11=[];
            g22=[];
            for j=1:w
                j
                r1111=[];
                r11=[];
                for i=1:8
                    r1111=[r1111 r1(:,i+(j-1)*8)-r1(:,i+j*8)];
                end    
                r11=[r11 r1111];
                for k=1:j-1
                    r11=[r11;-r1111];
                end
                r111=[];
                for i=1:8
                    r111=[r111 r1(:,i+j*8)];%g1=[r1(:,i)-r1(:,i+8);r1(:,i+8)];
                end
                g11=[g11 r11
                    zeros(2,(j-1)*8) r111];
%-------------------------------------------------第二類源
                r2222=[];
                r22=[];
                for i=1:8
                    r2222=[r2222 r2(:,i+(j-1)*8)-r2(:,i+j*8)];
                end    
                r22=[r22 r2222];
                for k=1:j-1
                    r22=[r22;-r2222];
                end
                r222=[];
                for i=1:8
                    r222=[r222 r2(:,i+j*8)];%g1=[r1(:,i)-r1(:,i+8);r1(:,i+8)];
                end
                g22=[g22 r22
                    zeros(2,(j-1)*8) r222];
            end    
            g1=g11/(c*Ts);
            g2=g22/(c*Ts);
            h=[1,-1];
            for j=2:w 
                h=[h zeros(j-1,1)
                   zeros(1,j-1) [1,-1]];
            end
    %end
    end
    Q1=ones(w,1)*ones(1,w)*L^2*var_f+h*h'*var_n;
    [a b]=size(Q1);
     Q=[];
     for i=1:8
          Q=[Q;zeros(a,b*(i-1)) Q1 zeros(a,b*(8-i))];
     end
    
     inv_J1=inv(g1*inv(Q)*g1');
     var_w1(1,t)=trace(inv_J1);
%--------------------------------------------------第二類源
     inv_J2=inv(g2*inv(Q)*g2');
     var_w2(1,t)=trace(inv_J2);
     
end
figure
xlim([0,60]) 
ylim([10^-2,10^1])
semilogy(w1,var_w1(1,:),w1,var_w2(1,:),'r-+')
xlabel('w')
ylabel('position estimation CRLB (meter)')
grid on