【论文】卫星定位算法（一）
将大地坐标系转换为直角坐标系函数：
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