mirror of https://github.com/gnss-sdr/gnss-sdr
57 lines
1.4 KiB
Mathematica
57 lines
1.4 KiB
Mathematica
|
function [Az, El, D] = topocent(X, dx)
|
||
|
|
%TOPOCENT Transformation of vector dx into topocentric coordinate
|
||
|
|
% system with origin at X.
|
||
|
|
% Both parameters are 3 by 1 vectors.
|
||
|
|
%
|
||
|
|
%[Az, El, D] = topocent(X, dx);
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% X - vector origin corrdinates (in ECEF system [X; Y; Z;])
|
||
|
|
% dx - vector ([dX; dY; dZ;]).
|
||
|
|
%
|
||
|
|
% Outputs:
|
||
|
|
% D - vector length. Units like units of the input
|
||
|
|
% Az - azimuth from north positive clockwise, degrees
|
||
|
|
% El - elevation angle, degrees
|
||
|
|
|
||
|
|
%Kai Borre 11-24-96
|
||
|
|
%Copyright (c) by Kai Borre
|
||
|
|
%
|
||
|
|
% CVS record:
|
||
|
|
% $Id: topocent.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $
|
||
|
|
%==========================================================================
|
||
|
|
|
||
|
|
dtr = pi/180;
|
||
|
|
|
||
|
|
[phi, lambda, h] = togeod(6378137, 298.257223563, X(1), X(2), X(3));
|
||
|
|
|
||
|
|
cl = cos(lambda * dtr);
|
||
|
|
sl = sin(lambda * dtr);
|
||
|
|
cb = cos(phi * dtr);
|
||
|
|
sb = sin(phi * dtr);
|
||
|
|
|
||
|
|
F = [-sl -sb*cl cb*cl;
|
||
|
|
cl -sb*sl cb*sl;
|
||
|
|
0 cb sb];
|
||
|
|
|
||
|
|
local_vector = F' * dx;
|
||
|
|
E = local_vector(1);
|
||
|
|
N = local_vector(2);
|
||
|
|
U = local_vector(3);
|
||
|
|
|
||
|
|
hor_dis = sqrt(E^2 + N^2);
|
||
|
|
|
||
|
|
if hor_dis < 1.e-20
|
||
|
|
Az = 0;
|
||
|
|
El = 90;
|
||
|
|
else
|
||
|
|
Az = atan2(E, N)/dtr;
|
||
|
|
El = atan2(U, hor_dis)/dtr;
|
||
|
|
end
|
||
|
|
|
||
|
|
if Az < 0
|
||
|
|
Az = Az + 360;
|
||
|
|
end
|
||
|
|
|
||
|
|
D = sqrt(dx(1)^2 + dx(2)^2 + dx(3)^2);
|
||
|
|
%%%%%%%%% end topocent.m %%%%%%%%%
|