%  本程序實現測試、比較無線定位算法

%    參數說明：
%       移動臺初始位置由MSPosition給出
%       輸出移動臺的估計位置。
%       比較各個算法的RMSE、CRLB、GDOP
%       比較各個算法的時間復雜度。

BSN = [7,6,5,4];            %基站數目
%BSN = 7;
R = 3000;                   %
CalNum = 10000;             %使用Monte Carlo法仿真

%幾種傳統定位算法的比較
%Noise = [30, 60, 90, 150, 210, 300];
Noise =90;
  n=1;
for m = 1:4
    %for n = 1:6
  
    for i = 1: CalNum
            MSP = MSPosition();     %移動臺初始位置
            MS(i,1) = MSP(1)*R;     %x坐標值
            MS(i,2) = MSP(2)*R;     %y坐標值

            EMSCI = ChanAlgorithm(BSN(m), MSP, R, Noise);%Chan算法定位
            EMSC(i, 1) = EMSCI(1);   %x坐標估計值
            EMSC(i, 2) = EMSCI(2);   %y坐標估計值

            EMSTI = TaylorAlgorithm(BSN(m), MSP, R, Noise);%Taylor算法定位
            EMST(i ,1) = EMSTI(1);   %x坐標估計值
            EMST(i, 2) = EMSTI(2);   %y坐標估計值

           % EMSSII = SIAlgorithm(BSN(m), MSP, R, Noise(n));
           % EMSSI(i ,1) = EMSSII(1);
           % EMSSI(i, 2) = EMSSII(2);

        end

        rmse_c(m) = TDOA_RMSE(MS,EMSC);         %Chan算法測試精度
        rmse_t(m) = TDOA_RMSE(MS,EMST);         %Taylor算法測試精度
       % rmse_si(m,n) = TDOA_RMSE(MS,EMSSI);
       % crlb(m) = CRLB(BSN(m), MSP, R, Noise);%理論測試精度
    %end
end
crlb(1:4)=[48,57,67,69];
figure(1)
plot(BSN,rmse_c,'b-*',BSN,rmse_t,'k-s',BSN,crlb,'r-o');
xlabel('基站個數N');
ylabel('RMSE(米)');
legend('Chan','Taylor','Crlb');
grid on;