blh_neu.m

BLH_NEU 将WGS-84系下的经纬高

function [X,Y,Z]=BLH_NEU(B,L,H,alpha)  

% BLH_NEU 将WGS-84系下的经纬高，转换到发射坐标系下的XYZ值。alpha表示发射角，用弧度表示
% 转换过程:(BLH)_wgs to (XYZ)_wgs to (XYZ)enu  to (XYZ)_fashe
% BLH ( WGS-84) －－－－发射系（和北东天直角系相差发射角 alpha）
%  B 纬度，L 经度，H  高度
% B(1:n)----要求列向量


n=size(B);
X(1:n,1)=0; Y(1:n,1)=0; Z(1:n,1)=0; 

a_WGS = 6378137;  alpha_WGS = 1/298.257223563;
e2_WGS = 2*alpha_WGS-alpha_WGS^2;
N_WGS = a_WGS./sqrt(1-e2_WGS.*sin(B(:,1)).^2);          % WGS84的长半轴和扁率,第一偏心率,卯酉圈半径
   
X(1:n,1) = (N_WGS+H(1:n,1)).*cos(B(1:n,1)).*cos(L(1:n,1)) ;
Y(1:n,1) = (N_WGS+H(1:n,1)).*cos(B(1:n,1)).*sin(L(1:n,1));
Z(1:n,1) = [N_WGS*(1-e2_WGS)+H(1:n,1)].*sin(B(1:n,1));    % WGS-84大地系－－－化为WGS-84直角系
   
A=[-sin(B(1,1))*cos(L(1,1))  -sin(B(1,1))*sin(L(1,1))  cos(B(1,1)) 
    -sin(L(1,1))  cos(L(1,1))  0;
    cos(B(1,1))*cos(L(1,1))  cos(B(1,1))*sin(L(1,1))  sin(B(1,1))];    % 旋转矩阵，

x0=X(1,1);y0=Y(1,1); z0=Z(1,1);    % 发射坐标系的原点

X1(1,1:n)=(X(1:n,1)-x0)';    % 平移后化为行向量
Y1(1,1:n)=(Y(1:n,1)-y0)';
Z1(1,1:n)=(Z(1:n,1)-z0)';


X(1:n,1)=(A(1,:)*[X1;Y1;Z1])';  % 
Y(1:n,1)=(A(2,:)*[X1;Y1;Z1])';
Z(1:n,1)=(A(3,:)*[X1;Y1;Z1])';   % 化为NEU下的XYZ

A(:,:)=0;
A=[cos(alpha)  -sin(alpha) 0;sin(alpha) cos(alpha) 0; 0 0 1];  % NEU和发射系的旋转阵

X(1:n,1)=(A(1,:)*[X';Y';Z'])';  % 化为发射坐标系下的XYZ
Y(1:n,1)=(A(2,:)*[X';Y';Z'])'; 
Z(1:n,1)=(A(3,:)*[X';Y';Z'])'; 




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%vv *** last line of BLH_NEU.m ***%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% 测试数据：B(1:4,1)=[20;21;22;23]; L(1:4,1)=[20;21;22;23];H(1:4,1)=[100;120;140;160]; alpha=pi/6;[X,Y,Z]=BLH_NEU(B,L,H,alpha)
% 结果：