最小二乘接收机位置计算

function [pos, el, az, dop] = leastSquarePos(satpos, obs)
%最小二乘接收机位置计算.
%
%[pos, el, az, dop] = leastSquarePos(satpos, obs, settings);
%
% 输入
% satpos - 卫星位置 (ECEF坐标系: [X; Y; Z;] -每颗卫星一列)
% obs - 伪距观测量
% (e.g. [20000000 21000000 .... .... .... .... ....])
%
% 输出:
% pos - 接收机位置和时钟误差(in ECEF system: [X, Y, Z, dt])
% el - 卫星仰角 (degrees)
% az - 卫星方位角 (degrees)
% dop - 精度因子 ([GDOP PDOP HDOP VDOP TDOP])

%--------------------------------------------------------------------------


% 初始化
settings.c = 299792458; % 光速 [m/s]
nmbOfIterations = 7;

dtr = pi/180;
pos = zeros(4, 1);
X = satpos;
nmbOfSatellites = size(satpos, 2);

A = zeros(nmbOfSatellites, 4);
omc = zeros(nmbOfSatellites, 1);
az = zeros(1, nmbOfSatellites);
el = az;

%=== 迭代计算接收机位置 ===================================
for iter = 1:nmbOfIterations

for i = 1:nmbOfSatellites
if iter == 1
%--- 第一次迭代时初始化变量 --------------
Rot_X = X(:, i);
trop = 2;
else
%--- 方程更新 -----------------------------------------
rho2 = (X(1, i) - pos(1))^2 + (X(2, i) - pos(2))^2 + ...
(X(3, i) - pos(3))^2;
traveltime = sqrt(rho2) / settings.c ;

%--- 改正卫星由于地球运动引起位置改变 --------
Rot_X = e_r_corr(traveltime, X(:, i));

%--- 找卫星仰角 ----------------
% [az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :));

trop = 0;%对流层改正为0
end % if iter == 1 ... ... else

%--- 改正 ----------------------------------------
omc(i) = (obs(i) - norm(Rot_X - pos(1:3), 'fro') - pos(4) - trop);

%---重构 A矩阵 ---------------------------------------
A(i, :) = [ (-(Rot_X(1) - pos(1))) / obs(i) ...
(-(Rot_X(2) - pos(2))) / obs(i) ...
(-(Rot_X(3) - pos(3))) / obs(i) ...
1 ];
end % for i = 1:nmbOfSatellites

% These lines allow the code to exit gracefully in case of any errors
if rank(A) ~= 4
pos = zeros(1, 4);
return
end

%--- 计算位置 ---------------------------------------------
x = A \ omc;

%--- 位置更新--------------------------------------------
pos = pos + x;

end % for iter = 1:nmbOfIterations

pos = pos';

%=== 计算几何因子======================================
if nargout == 4
%---初始化输出 ------------------------------------------------
dop = zeros(1, 5);

%--- 计算 DOP -------------------------

---------------------------
Q = inv(A'*A);

dop(1) = sqrt(trace(Q)); % GDOP
dop(2) = sqrt(Q(1,1) + Q(2,2) + Q(3,3)); % PDOP
dop(3) = sqrt(Q(1,1) + Q(2,2)); % HDOP
dop(4) = sqrt(Q(3,3)); % VDOP
dop(5) = sqrt(Q(4,4)); % TDOP
end


function X_sat_rot = e_r_corr(traveltime, X_sat)

%
%X_sat_rot = e_r_corr(traveltime, X_sat);
%
% 输入:
% travelTime - 信号传输时间
% X_sat - 卫星的 ECEF坐标
%
% 输出:
% X_sat_rot - 旋转的卫星坐标 (ECEF)



Omegae_dot = 7.292115147e-5; % rad/sec

%--- 旋转角 --------------------------------------------------
omegatau = Omegae_dot * traveltime;

%--- 旋转矩阵 -----------------------------------------------
R3 = [ cos(omegatau) sin(omegatau) 0;
-sin(omegatau) cos(omegatau) 0;
0 0 1];

%--- 旋转 ------------------------------------------------------
X_sat_rot = R3 * X_sat;