forked from ZhanLi/UWBIns
44 lines
792 B
Mathematica
44 lines
792 B
Mathematica
|
function [X, dx, G] = ch_gpsls(X, sat_pos, pr)
|
|||
|
|
% GPS α<EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD>˷<EFBFBD><EFBFBD><EFBFBD><EFBFBD>⣬ ״̬<EFBFBD><EFBFBD>Ϊ X Y Z B(<EFBFBD>Ӳ<EFBFBD>)
|
|||
|
|
% X: X ֵ(1:3) λ<EFBFBD><EFBFBD>delta, (4) <EFBFBD>û<EFBFBD><EFBFBD>Ӳ<EFBFBD>ƫ<EFBFBD><EFBFBD>
|
|||
|
|
% G: <EFBFBD><EFBFBD><EFBFBD>ƾ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
% pr: У<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>α<EFBFBD><EFBFBD>
|
|||
|
|
% sat_pos: <EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>þ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
% delta: delta ֵ(1:3) λ<EFBFBD><EFBFBD>delta, (4) <EFBFBD>û<EFBFBD><EFBFBD>Ӳ<EFBFBD>ƫ<EFBFBD><EFBFBD>
|
|||
|
|
|
|||
|
|
B1=1;
|
|||
|
|
END_LOOP=100;
|
|||
|
|
%<EFBFBD><EFBFBD><EFBFBD>Ǹ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
n = size(sat_pos, 2);
|
|||
|
|
|
|||
|
|
if n < 4
|
|||
|
|
dx = 0;
|
|||
|
|
G = 0;
|
|||
|
|
return
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
for loop = 1:10
|
|||
|
|
% <EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD>ǰλ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>վ<EFBFBD>ľ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
r = vecnorm(sat_pos - X(1:3));
|
|||
|
|
|
|||
|
|
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD>H<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
H = (sat_pos - X(1:3)) ./ r;
|
|||
|
|
H =-H';
|
|||
|
|
|
|||
|
|
H = [H(:,1:3), ones(n,1)];
|
|||
|
|
|
|||
|
|
dp = ((pr - r) - X(4))';
|
|||
|
|
|
|||
|
|
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
dx = (H'*H)^(-1)*H'*dp;
|
|||
|
|
X = X + dx;
|
|||
|
|
G = H;
|
|||
|
|
|
|||
|
|
% END_LOOP = vnorm(delta(1:3));
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
|