44 lines
792 B
Matlab
44 lines
792 B
Matlab
|
function [X, dx, G] = ch_gpsls(X, sat_pos, pr)
|
|||
|
|
% GPS α<><CEB1><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD>˷<EFBFBD><CBB7><EFBFBD><EFBFBD>⣬ ״̬<D7B4><CCAC>Ϊ X Y Z B(<28>Ӳ<EFBFBD>)
|
|||
|
|
% X: X ֵ(1:3) λ<><CEBB>delta, (4) <20>û<EFBFBD><C3BB>Ӳ<EFBFBD>ƫ<EFBFBD><C6AB>
|
|||
|
|
% G: <20><><EFBFBD>ƾ<EFBFBD><C6BE><EFBFBD>
|
|||
|
|
% pr: У<><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>α<EFBFBD><CEB1>
|
|||
|
|
% sat_pos: <20><><EFBFBD><EFBFBD>λ<EFBFBD>þ<EFBFBD><C3BE><EFBFBD>
|
|||
|
|
% delta: delta ֵ(1:3) λ<><CEBB>delta, (4) <20>û<EFBFBD><C3BB>Ӳ<EFBFBD>ƫ<EFBFBD><C6AB>
|
|||
|
|
|
|||
|
|
B1=1;
|
|||
|
|
END_LOOP=100;
|
|||
|
|
%<25><><EFBFBD>Ǹ<EFBFBD><C7B8><EFBFBD>
|
|||
|
|
n = size(sat_pos, 2);
|
|||
|
|
|
|||
|
|
if n < 4
|
|||
|
|
dx = 0;
|
|||
|
|
G = 0;
|
|||
|
|
return
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
for loop = 1:10
|
|||
|
|
% <20><><EFBFBD>õ<EFBFBD>ǰλ<C7B0><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>վ<EFBFBD>ľ<EFBFBD><C4BE><EFBFBD>
|
|||
|
|
r = vecnorm(sat_pos - X(1:3));
|
|||
|
|
|
|||
|
|
% <20><><EFBFBD><EFBFBD>H<EFBFBD><48><EFBFBD><EFBFBD>
|
|||
|
|
H = (sat_pos - X(1:3)) ./ r;
|
|||
|
|
H =-H';
|
|||
|
|
|
|||
|
|
H = [H(:,1:3), ones(n,1)];
|
|||
|
|
|
|||
|
|
dp = ((pr - r) - X(4))';
|
|||
|
|
|
|||
|
|
% <20><><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
dx = (H'*H)^(-1)*H'*dp;
|
|||
|
|
X = X + dx;
|
|||
|
|
G = H;
|
|||
|
|
|
|||
|
|
% END_LOOP = vnorm(delta(1:3));
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
|