%?Z.?Ma?and?K.?C.?Ho?“TOA?localization?in?the?presence?of?random?sensor?
%?position?errors“?in?2011?IEEE?International?Conference?on?Acoustics?
%?Speech?and?Signal?Processing?（ICASSP）?2011?pp.?2468-2471.

%?by?Y.?Sun?UESTC?2018.12.15

clear;close?all;clc;

senPos?=?[?300?100?150
???????????400?150?100
???????????300?500?200
???????????350?200?100
???????????-100?100?-100
???????????200?-300?-200]‘;
souLoc?=?[[2000;2500;3000][600;650;550]];

nsePwr?=?10^（-10/10）;
errPwrdB?=?-60:10:0;
errPwr?=?10.^（errPwrdB/10）;

[NM]?=?size（senPos）;
mon?=?1000;
%?measurements?noise
rng（‘Default‘）;
nseTmp?=?zeros（Mmon）;
for?m?=?1:mon
????nseTmp（:m）?=?randn（M1）;
end
nseTmp?=?nseTmp?-?mean（nseTmp2）;

%?sensor?position?errors
rng（12）;
errTmp?=?zeros（M*N1）;
for?m?=?1:mon
????errTmp（:m）?=?randn（M*N1）;
end
errTmp?=?errTmp?-?mean（errTmp2）;

Qr?=?nsePwr?*?eye（M）;
J?=?diag（[1?1?1?2?2?2?10?10?10?40?40?40?20?20?20?3?3?3]）;

%?distant?source
ro（:1）?=?sqrt（sum（（souLoc（:1）-senPos）.^21））‘;
ro（:2）?=?sqrt（sum（（souLoc（:2）-senPos）.^21））‘;

for?u?=?1:2
????for?n?=?1:length（errPwr）
????????Qs?=?errPwr（n）*J;
????????
????????%?CRLB
????????for?i?=?1:M
????????????du（i:）?=?（souLoc（:u）-senPos（:i））‘./ro（iu）;
????????????ds（i（1:N）+（i-1）*N）?=?-（souLoc（:u）-senPos（:i））‘./ro（iu）;
????????end
????????X?=?du‘/Qr*du;
????????Y?=?du‘/Qr*ds;
????????Z?=?ds‘/Qr*ds?+?inv（Qs）;
????????CRB?=?inv（X）?+?X\Y/（Z-Y‘/X*Y）*Y‘/X;
????????crlb（un）?=?trace（CRB）;
????????
????????%?simulation
????????for?m?=?1:mon
????????????r?=?ro（:u）?+?sqrt（Qr）*nseTmp（:m）;
????????????senPosNsed?=?senPos?+?reshape（chol（Qs）‘*errTmp（:m）NM）;
????????????
%?????????????h1?=?r.^2?-?diag（senPosNsed‘*senPosNsed）;
%?????????????G1?=?-2*[senPosNsed‘-0.5*ones（M1）];
%?????????????psi1?=?（G1‘*G1）\G1‘*h1;
%?????????????
%?????????????V1?=?2*diag（r）;
%?????????????for?s?=?1:M
%?????????????????O1（s（1:N）+（s-1）*N）?=?（psi1（1:N）-senPosNsed（:s））‘;
%?????????????end
%?????????????W1?=?inv（V1*Qr*V1‘+O1*Qs*O1‘）;
%?????????????psi2?=?（G1‘*W1*G1）\G1‘*W1*h1;
%?????????????C1?=?inv（G1‘*W1*G1）;
%?????????????
%?????????????h2?=?[psi2（1:N）.^2;psi2（4）];
%?????????????G2?=?[eye（N）;ones（1N）];
%?????????????V2?=?diag（[2*psi2（1:N）;1]）;
%?????????????W2?=?inv（V2*C1*V2‘）;
%?????????????psi3?=?（G2‘*W2*G2）\G2‘*W2*h2;
%?????????????
%?????????????u1e?=?sign（psi2（1:N））.*sqrt（psi3）;
????????????u1e?=?TOA_2WLS_Cart（senPosNsedrQrQs）;
????????????err1（m）?=?norm（u1e-souLoc（:u））;
????????end
????????mse（un）?=?mean（err1.^2）;
????end
end

figure;
plot（errPwrdB10*log10（mse（1:））‘o‘‘linewidth‘1.5‘DisplayName‘‘distant‘）;hold?on;grid?on;
plot（errPwrdB10*log10（mse（2:））‘^‘‘linewidth‘1.5‘DisplayName‘‘near‘）;
plot（errPwrdB10*log10（crlb（1:））‘-‘‘linewidth‘1.5‘DisplayName‘‘CRLB-distant‘）;
plot（errPwrdB10*log10（crlb（2:））‘--‘‘linewidth‘1.5‘DisplayName‘‘CRLB-near‘）;
legend（‘show‘‘location‘‘best‘）