[논문] 위성 포 지 셔 닝 알고리즘 (1)

대지 좌표 계 를 직각 좌표계 함수 로 변환:

function [ x,y,z ] = WGS_84(L,B,H) 
%UNTITLED3              
%            
%WGS_84     (L,B,H)          (x,y,z) 
% e2            
% re         
% N         
BR = B*pi/180; 
LR = L*pi/180; 
e2 = 0.00669437999013; 
re = 6378137;%   m 
N = re /sqrt(1 - e2 * sin(BR) * sin(BR)); 
x = (N + H) * cos(BR) * cos(LR); 
y = (N + H) * cos(BR) * sin(LR); 
z = (N * (1 - e2) + H ) * sin(BR); 
end

직각 좌 표를 대지 좌표계 함수 로 변환:

function [ L,B ] = r_WGS_84( xx,yy,zz ) 
%UNTITLED              
%            
% xx = uuu(1); 
% yy = uuu(2); 
% zz = uuu(3); 
e2 = 0.00669437999013; 
re = 6378137;%   m 
L = atan (yy/xx); 
 
B  = atan(zz*sin(L)/(yy*(1-e2))); 
% N = re /sqrt(1 - e2 * sin(B) * sin(B)); 
% B = atan( zz*(N+H) / (sqrt(xx*xx + yy*yy)*(N*(1 - e2)+H) )); 
% H = yy/(cos(B)*sin(L))-N; 
end 

포 지 셔 닝 알고리즘:

clc 
clear all 
r = 6367.287e+03; 
% r2 = r * r; 
height = 42164e+03; 
H = height - r; 
[x1,y1,z1] = WGS_84(50.0 , 2.0 , H ); 
s1 = [x1,y1,z1]'; 
[x2,y2,z2] = WGS_84(51.5 , 3.0 , H ); 
s2 = [x2,y2,z2]'; 
[x3,y3,z3] = WGS_84(53.0 , 0.0 , H ); 
s3 = [x3,y3,z3]'; 
%         0       u 
[x,y,z] = WGS_84(60.5 , 10 , 0 ); 
u = [x,y,z]' 
%             
[xr,yr,zr] = WGS_84(55,6.0,0); 
rec = [xr,yr,zr]'; 
 
r11 = sqrt((s1 - u)'*(s1 - u)); 
r22 = sqrt((s2 - u)'*(s2 - u)); 
r33 = sqrt((s3 - u)'*(s3 - u)); 
c = 3.0e+08; 
rm1 = sqrt((s1 - rec)'*(s1 - rec)); 
rm2 = sqrt((s2 - rec)'*(s2 - rec)); 
rm3 = sqrt((s3 - rec)'*(s3 - rec)); 
 
TDOA1 = (((r22+rm2)-(r11+rm1))/c);%20ms 
TDOA2 = (((r33+rm3)-(r11+rm1))/c);%30e-09; 
 
r21 = c * TDOA1 + (rm1 -rm2) ; 
r31 = c * TDOA2 + (rm1 -rm3) ; 
syms r1 
j1 = -2*[s1';s2'-s1';s3'-s1']; 
h = [-r*r-s1'*s1             , 0     , 1; 
     r21*r21-s2'*s2+s1'*s1  , 2*r21 , 0; 
     r31*r31-s3'*s3+s1'*s1  , 2*r31 , 0] *  [1;r1;r1*r1]; 
 uu = j1 \ h ; 
 y = uu' * uu - r * r; 
 b = solve(y); 
 max_b = max(b); 
%  b(1) 
 uuu =  subs( uu,r1,b(2) ) 
 [LU,BU] =  r_WGS_84(uuu(1),uuu(2),uuu(3)); 
 E_UL = LU * 180 /3.1415926 
 E_UB = BU * 180 /3.1415926 
 RMSE_U = sqrt((E_UL-51.5)^2 + (E_UB-1.0)^2) 

결과:

u =

   1.0e+06 *

    3.0933
    5.4675
    1.1002

 
uuu =
 
 3086570.9670260144338753183709831
 5459370.8270755531923585929203801
 1100315.2177251228042065562449973
 
 
E_UL =
 
60.517434133654494674580042266545
 
 
E_UB =
 
10.016807540300662682260165017612
 
 
RMSE_U =
 
12.752134588907853650012948113517