TOA定位两步加权最小二乘MATLAB算法程序

本人翻遍了CSDN都找不到一个正确的TOA定位算法程序，唯一找到的一个是用最小二乘解的（参考文献N. Patwari, J. N. Ash, S. Kyperountas, A. O. Hero, R. L. Moses, and N. S. Correal, "Locating the nodes: cooperative localization in wireless sensor networks," IEEE Signal Processing Magazine, vol. 22, no. 4, pp. 54-69, 2005.），性能无法达到克拉美罗界。 因此本人自己重新写了一个程序，参考该领域著名学者K. C. Ho的文章（参考文献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.）。该算法适用于传感器位置有误差/无误差的情况，算法性能能够达到克拉美罗界。示例程序中给出了CRLB的程序，场景为传感器有误差的情况。程序运行结果与参考文献一致。 （搞不懂现在的人都是要什么50积分，多分享下不好吗？） ******特别提示******：本代码多处使用了Matlab 2016a以后支持的新语法，旧版本无法正常运行的，请自行修改代码或更新Matlab版本！！！

% 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‘）

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        2993  2018-12-15 19:35  testTOAErrCartesian.m
     文件        1446  2018-12-15 15:04  TOA_2WLS_Cart.m