伪距定位算法
Posted 者乎之类的
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了伪距定位算法相关的知识,希望对你有一定的参考价值。
伪距定位算法:
1.准备数据与设置初始解:准备卫星位置(经地球自转校正后)和伪距值
设置接收机初始位置、接收机钟差
2.非线性方程线性化
3.求解非线性方程
4.更新非线性方程的根
5.迭代完毕
function [pos, el, az, dop] = leastSquarePos(satpos, obs, settings)
%satpos卫星坐标xyz;obs观测伪距
%Function calculates the Least Square Solution.
%
%[pos, el, az, dop] = leastSquarePos(satpos, obs, settings);
%
% Inputs:
% satpos - Satellites positions (in ECEF system: [X; Y; Z;] -
% one column per satellite)
% obs - Observations - the pseudorange measurements to each
% satellite:
% (e.g. [20000000 21000000 .... .... .... .... ....])
% settings - receiver settings
%
% Outputs:
% pos - receiver position and receiver clock error
% (in ECEF system: [X, Y, Z, dt])
% el - Satellites elevation angles (degrees)
% az - Satellites azimuth angles (degrees)
% dop - Dilutions Of Precision ([GDOP PDOP HDOP VDOP TDOP])
%--------------------------------------------------------------------------
% SoftGNSS v3.0
%--------------------------------------------------------------------------
%Based on Kai Borre
%Copyright (c) by Kai Borre
%Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen
%
% CVS record:
% $Id: leastSquarePos.m,v 1.1.2.12 2006/08/22 13:45:59 dpl Exp $
%==========================================================================
%=== Initialization =======================================================
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;
%=== Iteratively find receiver position ===================================
for iter = 1:nmbOfIterations
for i = 1:nmbOfSatellites
if iter == 1
%--- Initialize variables at the first iteration --------------
Rot_X = X(:, i);
trop = 2; % 对流层矫正值
else
%--- Update equations -----------------------------------------
rho2 = (X(1, i) - pos(1))^2 + (X(2, i) - pos(2))^2 + ...
(X(3, i) - pos(3))^2;%伪距预测值,即r
traveltime = sqrt(rho2) / settings.c ;
%--- Correct satellite position (do to earth rotation) --------
Rot_X = e_r_corr(traveltime, X(:, i));
%X是(t-tao)(卫星发射信号)时刻的在WGS-84坐标系下的卫星位置;
%Rot_X是t(接收机接收到信号)时刻在WGS-84坐标下的卫星位置
%两者差别很大倒不是因为卫星绝对位置移动了多少,而是地球自转导致坐标系移动了
%--- Find the elevation angel of the satellite ----------------
[az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :));
%根据接收机位置和伪距矢量计算方位角、高度角、距离
if (settings.useTropCorr == 1)
%--- Calculate tropospheric correction --------------------
trop = tropo(sin(el(i) * dtr), ...
0.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
else
% Do not calculate or apply the tropospheric corrections
trop = 0;
end
end % if iter == 1 ... ... else
%--- Apply the corrections ----------------------------------------
omc(i) = (obs(i) - norm(Rot_X - pos(1:3), 'fro') - pos(4) - trop);
% n = norm(v) 返回向量 v 的欧几里德范数。此范数也称为 2-范数、向量模或欧几里德长度。
%omc就是b
%--- Construct the A matrix ---------------------------------------
A(i, :) = [ (-(Rot_X(1) - pos(1))) / obs(i) ... %Rot_X是卫星的位置,pos是接收机大体位置
(-(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
%--- Find position update ---------------------------------------------
x = A \\ omc; %只用了4颗卫星,所以没有用最小二乘;
% \\
% 对线性方程组 Ax = B 求解 x
%
% 此 MATLAB 函数 对线性方程组 A*x = B 求解。矩阵 A 和 B 必须具有相同的行数。如果 A 未正确缩放或接近奇异值,MATLAB
% 将会显示警告信息,但还是会执行计算。
%
% x = A\\B
% x = mldivide(A,B)
%--- Apply position update --------------------------------------------
pos = pos + x;
end % for iter = 1:nmbOfIterations
pos = pos';
%=== Calculate Dilution Of Precision ======================================
if nargout == 4
%--- Initialize output ------------------------------------------------
dop = zeros(1, 5);
%--- Calculate 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
以上是关于伪距定位算法的主要内容,如果未能解决你的问题,请参考以下文章