herror.m

求解定位方程组。并直接利用三元方程求出X,Y,Z时的误差子函数

%直接利用三元方程求出X,Y,Z时的误差子函数%
function  YYY=aerror(X,dx) 
global xx
global A
global R
global aa
global Z0
%load ss;  %不用global定义变量，而是直接导入数据，再赋给xx,A,R,X0%
dr=[0.001 0.002]; % 距离误差
da=[0.1 0.5 1]./180.*pi;  %测角误差
%dx=[5 10]; %位置坐标误差
xx1=xx;
if xx1(1,1)>xx1(2,1) 
    xx1=[xx1(2,1:3);xx1(1,1:3)];
    %a=[a(2,1);a(1,1)];
    %R=[R(2,1);R(1,1)]; 
end
R1=sqrt((X(1)-xx1(:,1)).^2+(X(2)-xx1(:,2)).^2+(X(3)-xx1(:,3)).^2);
l1=sqrt((X(1)-xx1(1,1)).^2+(X(2)-xx1(1,2)).^2);
l2=sqrt((X(1)-xx1(2,1)).^2+(X(2)-xx1(2,2)).^2);

a1=asin((X(1)-xx1(1,1))/l1);
a2=asin((X(1)-xx1(2,1))/l2);
a=[a1 a2];
Z1=X(1,3);
for k=1:2
    for m=1:1:3
       for n=1:100
          %R=R1.*(1+(rand(1,1)-0.5)*2*dr(k));
          R(1,1)=R1(1)*(1+(rand(1,1)-0.5)*2*dr(k));
          R(2,1)=R1(2)*(1+(rand(1,1)-0.5)*2*dr(k));
          %a0=a.*(1+(rand(1,1)-0.5)*2*da(m));
          a11=a1+(rand(1,1)-0.5)*2*da(m);
          a22=a2+(rand(1,1)-0.5)*2*da(m);
          aa=a11-a22;
          %aa=a0(1)-a0(2);
          xx=xx1+(rand(1,1)-0.5)*2*dx;
          X01=[8400 -85 4845];
          yyy1=fsolve('fc4',X01,1.0e-8);
          XYZ1(n,:,m,k)=yyy1;
          yy1=abs(yyy1-X);
          %drr(n,m,k)=sqrt(yy(1)^2+yy(2)^2); % 水平面上距离r的误差%
          %dRR(n,m,k)=sqrt(sum(yy.^2));% 空间上R的误差
          wcc1(n,:,m,k)=abs(yy1);  % 从前往后依次是行，列，页，第四维%
          Z0=Z1+(rand(1,1)-0.5)*2*15;
          %xx=xx1+(rand(1,1)-0.5)*2*dx;
          X02=[8400 -85];
          yy0=fsolve('fcn',X02,1.0e-6);
          yy2=[yy0 Z1];  
          XYZ2(n,:,m,k)=yy0;
          yy2=abs(yy0-X(1,1:2));
          %drr(n,k)=sqrt(yy(1)^2+yy(2)^2); % 水平面上距离r的误差
          wcc2(n,:,m,k)=abs(yy2); 
       end
     DXYZ1(:,m,k)=(std(XYZ1(:,:,m,k)))'; %第一行是X的偏差，第二行是Y的偏差
                        %第一页和第二页分别是k等于1和2时的偏差          
     dwcc1(:,m,k)=[sum(wcc1(:,1,m,k));sum(wcc1(:,2,m,k));...
             sum(wcc1(:,3,m,k))]./100; % X,Y,Z的平均误差,求和再平均后得到的%
     ddrr1(1,m,k)=sqrt((dwcc1(1,m,k))^2+(dwcc1(2,m,k))^2); % r的平均误差
     DXYZ2(:,m,k)=(std(XYZ2(:,:,m,k)))'; %第一行是X的偏差，第二行是Y的偏差
                        %第一页和第二页分别是k等于1和2时的偏差          
     dwcc2(:,m,k)=[sum(wcc2(:,1,m,k));sum(wcc2(:,2,m,k))]./100; % X,Y,Z的平均误差,求和再平均后得到的%
     ddrr2(:,m,k)=sqrt(sum((dwcc2(:,m,k)).^2));
   end
end
YYY=[DXYZ1;ddrr1;DXYZ2;ddrr2];