Demo entry 6327122

matlab m file

   

Submitted by Samantha on Nov 23, 2016 at 19:11
Language: Matlab. Code size: 3.1 kB.

clear
clc

%%% X Y Z Range

%%% 数据表1
A=[-10201908.19	21391461.63	-11524653.73	24434992.16
    14834389.78	7027967.845	21026271.48	24163990.9
-15181971.95	-10290286.19	19192930.9	24753152.67
    -9021052.94	17564432.62	17512706.19	20027824.97
  14682456.19	17166876.49	13855849.15	23314629.55
-12816859.16	10207024.92	21131470.46	20926250.25
-19935527.91	-60076.1591	17575386.83	22739806.85
-21545201.22	15380682.82	-354454.5898	22705397.91];

%%% 例题验算【可删
A=[-23517276.47 -8441337.22 9266091.87 25142929.85
    827058.89 24219485.57 10199910.09 21054624.95
    -12796270.59 17283306.26 15279722.70 20024645.90
    13190124.89 21040942.63 9630736.62 23776922.74
    -19028937.05 -630447.92 18670658.66 22900110.44
    -20972725.27 13817010.49 -8692192.55 23545705.70];

%%% 数据表2
A=[-10199959.87	21390257.57	-11528641.61	24436061.08
 14837401.97	7025380.215	21025066.06	24164983.48
-15181474.93	-10286652.83	19195250.19	24752154.97
-9020268.745	17567734.92	17509794.26	20027778
 14682268.41	17164156.25	13859363.31	23314476.33
-12813208.41	10208532.08	21132927.45	20926002.6
-19938034.41	-57865.7829	17572536.02	22739640.09
-21545062.34	15381049.89	-349632.4114	22704516.81];

x0=[0,0,0,0]';
dx0=[1,1,1,1]';
k=0;

while norm(dx0)>0.0001 
    for i=1:8
    DX=x0(1)-A(i,1);
    DY=x0(2)-A(i,2);
    DZ=x0(3)-A(i,3);
    DL=power(DX^2+DY^2+DZ^2,1/2);
    
    DR(i,1)=A(i,4)-DL-2.99792458*power(10,5)*x0(4);
    H(i,:)=[DX/DL,DY/DL,DZ/DL,2.99792458*power(10,5)];
    end
    dx0=(H'*H)\(H'*DR)
    x1=x0+dx0;
    x0=x1
    cb0=2.99792458*power(10,5)*x0(4) %% 可删
    ndx0=norm(dx0) %% 可删
    k
    k=k+1;
    X0t(k,:)=x0'; %% 矩阵形式并列输出多次迭代结果
end

x=x0(1);y=x0(2);z=x0(3);
longitude=atan(y/x)/pi*180;%% 经度

a=6378137;
e2=1/298.257*(2-1/298.257);
p=power(x^2+y^2,1/2);

h=0;
RN=a;
kk=0;

while 1
    sph0=z/((1-e2)*RN+h);%% sph0(RN,h)
    phi=atan((z+e2*RN*sph0)/p);

    sph1=sin(phi); %% sph1(phi)
    RN=a/power(1-e2*sph1^2,1/2);
    h=p/cos(phi)-RN;
    
    if abs(sph1-sph0)<power(10,-15)
        break
    end
    
    kk=kk+1
    RN_h=[RN,h] %% 可删
    phi %% 可删
    RN_h_phi(kk,:)=[RN,h,phi/pi*180]; %% 矩阵形式并列输出多次迭代结果
end

%%% 8选7
%%% 生成八星H阵
for i=1:8           
    DX(i,1)=x-A(i,1);
    DY(i,1)=y-A(i,2);
    DZ(i,1)=z-A(i,3);
    DL(i,1)=power(DX(i,1)^2+DY(i,1)^2+DZ(i,1)^2,1/2);
    
    H(i,:)=[DX(i,1)/DL(i,1),DY(i,1)/DL(i,1),DZ(i,1)/DL(i,1),2.99792458*power(10,5)];
end

%%% 减行生成七星HJ阵
for j=1:8 %%%去掉的卫星序号
    j
    
    if j==1
        disp('j=1');
    else
        for i=1:j-1             %%% j之前        
            HJ(i,:)=H(i,:);
        end
    end
    
    if j==8
        disp('j=8');
    else
        for i=j+1:8             %%% j之后
            HJ(i-1,:)=H(i,:);
        end
    end
    
    INV_HJTHJ=inv(HJ'*HJ);
    h11_h22_h33=[INV_HJTHJ(1,1),INV_HJTHJ(2,2),INV_HJTHJ(3,3)] %% 可删
    h11(j,1)=INV_HJTHJ(1,1);
    h22(j,1)=INV_HJTHJ(2,2);
    h33(j,1)=INV_HJTHJ(3,3);
    PDOP(j,1)=power( h11(j,1)+ h22(j,1)+ h33(j,1),1/2);
    clear HJ;
end

This snippet took 0.01 seconds to highlight.

Back to the Entry List or Home.

Delete this entry (admin only).