求大佬指点

我根据论文《***A Simple and Efficient Estimator for Hyperbolic Location***》用matlab仿真TDOA定位的经典算法Chan算法。
但是仿真结果一直不对。
主要问题：
目标定位的RMSE与CRLB不相等，但是估计的协方差矩阵与CRLB相等，找了很久都没有找到问题所在。请大佬指教！！！
代码如下：

clc;
close all;
clear all;
%***************
% 如果改变目标位置则不能达到CRLB？？？？？
%%
% 测向站数量
M_s=9;
% 6个测向站的位置坐标,单位米
s1=[0 0].';
s2=[-5 8].';
s3=[4 6].';
s4=[-2 4].';
s5=[7 3].';
s6=[-7 5].';
s7=[2 5].';
s8=[-4 2].';
s9=[3 3].';

s=zeros(3*M_s,1);
for i=1:1:M_s
    %     组合字符串，得到接收站的变量名，以便循环
    str_si=['s',mat2str(i)];
    %    返回变量名对应的值
    si_value=eval(str_si);
    s((i-1)*2+1:(i-1)*2+2,1)=si_value;
end
% 目标位置,单位米
u=[-50 250].';
%%
deta_s_more=10.^[-3:0.2:-1.2];
big_loop_number=length(deta_s_more);
small_loop_number=2000;
for big_loop=1:1:big_loop_number
    for small_loop=1:1:small_loop_number
        %距离差噪声
        deta_r=deta_s_more(big_loop);
        r_error=normrnd(0,deta_r,M_s-1,1);
        Qt=(deta_r^2)*(ones(M_s-1,M_s-1)+eye(M_s-1,M_s-1))/2;
        %% 无噪声矩阵
        % 距离差 无噪声
        ri1_0=zeros(M_s-1,1);
        for i=2:1:M_s
            str_si=['s',mat2str(i)];
            si_value=eval(str_si);
            ri1_0(i-1,1)=osjl(si_value,u)-osjl(s1,u);
        end
        
        Ri=zeros(M_s,1);
        for i=1:1:M_s
            str_si=['s',mat2str(i)];
            si_value=eval(str_si);
            Ri(i,1)=osjl(si_value,zeros(2,1));
        end
        G=zeros(M_s-1,3);
        h=zeros(M_s-1,1);
        for i=2:1:M_s
            str_si=['s',mat2str(i)];
            si_value=eval(str_si);
            G(i-1,:)=-1*[(si_value.'-s1.'),ri1_0(i-1,1)];
            h(i-1,1)=0.5*((ri1_0(i-1,1))^2-(Ri(i,1))^2+(Ri(1,1))^2);
        end
        result1=h-G*[u;osjl(s1,u)];
        %% 参数叠加噪声
        % 观测站和距离差加上误差
        ri1=ri1_0+r_error;
        %% 有噪声的矩阵 第一阶段
        G=zeros(M_s-1,3);
        h=zeros(M_s-1,1);
        for i=2:1:M_s
            str_si=['s',mat2str(i)];
            si_value=eval(str_si);
            G(i-1,:)=-1*[(si_value.'-s1.'),ri1(i-1,1)];
            h(i-1,1)=0.5*((ri1(i-1,1))^2-(Ri(i,1))^2+(Ri(1,1))^2);
        end       
        %         最初阶段
        za11=inv(G.'*inv(Qt)*G)*G.'*inv(Qt)*h;
        za1=za11(1:2);
        %% 第一阶段
        ri_=zeros(M_s-1,1);
        for i=2:1:M_s
            str_si=['s',mat2str(i)];
            si_value=eval(str_si);
            ri_(i-1,1)=osjl(za1,si_value);
        end
        B_=diag(ri_);
        W1=inv(B_*Qt*B_);
        
        za21=inv(G.'*W1*G)*G.'*W1*h;
        za22=za21(1:2);
        cov_za2=inv(G.'*W1*G);
        %% 第二阶段
        hs=[za22-s1;za21(3)].^2;
        Gs=[1 0;0 1;1 1];
        Ga_=[1,0;0,1;1,1];
        Bs=diag([za22-s1;za21(3)]);
        W=inv(4*Bs*cov_za2*Bs);
        
        za3=inv(Gs.'*W*Gs)*Gs.'*W*hs;
        estimate=[-1 0;0 1]*sqrt(abs(za3))+s1;
        cov_za3=inv(Gs.'*W*Gs);
        %% 协方差矩阵
        B__=diag([estimate-s1]);
        cov_stage2=0.25*inv(B__)*cov_za3*inv(B__);
         %%  
        MSE(big_loop,small_loop)=(sum((estimate-u).^2));
        MSE_cov_stage2(big_loop,small_loop)=trace(cov_stage2);
        [big_loop,small_loop]
    end
end
RMSE=10.*log10(sum(MSE,2)/small_loop_number);
RMSE_cov_stage2=10.*log10(sum(MSE_cov_stage2,2)/small_loop_number);
plot(10*log10(deta_s_more),(RMSE),'b.-')
hold on;
plot(10*log10(deta_s_more),(RMSE_cov_stage2),'g.-')
hold on;
%% 求CRLB
% 测向站数量
M=9;
% 目标数量
N=1;
% 光速m/s
c=3*10^8;
% 6个测向站的位置坐标,单位米
s1=[0 0].';
s2=[-5 8].';
s3=[4 6].';
s4=[-2 4].';
s5=[7 3].';
s6=[-7 5].';
s7=[2 5].';
s8=[-4 2].';
s9=[3 3].';
% 目标位置,单位米
u=[-50 250].';

r_diff_u=zeros(M-1,2);
for i=2:M
    %     组合字符串，得到接收站的变量名，以便循环
    str_si=['s',mat2str(i)];
    %    返回变量名对应的值
    si_value=eval(str_si);
    
    pi=(si_value-u)/osjl(u,si_value);
    p1=(s1-u)/osjl(u,s1);
    
    r_diff_u(i-1,:)=p1-pi;
    si_s1=(si_value-s1);
end

deta_s_more=10.^[-3:0.2:-1.2];
for loop=1:1:length(deta_s_more)
    deta_r=deta_s_more(loop);
    Qr=deta_r^2*(ones(M-1,M-1)+eye(M-1,M-1))/2;
    
    X=r_diff_u.'*inv(Qr)*r_diff_u;
    CRLB=inv([X]);
    CRLB_u(loop)=10.*log10(trace(CRLB));
end
hold on;
plot(10*log10(deta_s_more),CRLB_u,'r.-')
% ylim([0 60])
legend('RMSE','协方差矩阵','CRLB')

其中函数“osjl”是用来求两个点之间的距离

输出结果为：