www.gusucode.com > 扩展卡尔曼滤波源码程序 > 扩展卡尔曼滤波源码程序/EKF/EKF/Rcv_Pos_Compute.m
% Least squares method for GPS positioning. function [Rcv_Pos,Rcv_b] = Rcv_Pos_Compute(SV_Pos, SV_Rho) Num_Of_SV=size(SV_Pos); if Num_Of_SV<4 Rcv_Pos=[0 0 0];Rcv_b=0; return; end %% %Initial GPS Position Rcv_Pos=[0 0 0];Rcv_b=0; %Start Iteration %Constrain for convergence B1=1; END_LOOP=100; count=0; while (END_LOOP > B1) %A.Compute G G = G_Compute(SV_Pos, Rcv_Pos); %B.Compute Delta_Rho Delta_Rho = Delta_Rho_Compute(SV_Rho, SV_Pos, Rcv_Pos, Rcv_b); %C.Iteration for new postion Delta_X = inv(G' * G) * G' * Delta_Rho; Rcv_Pos = (Rcv_Pos' + Delta_X(1:3))'; Rcv_b = Rcv_b + Delta_X(4); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% END_LOOP = (Delta_X(1)^2 + Delta_X(2)^2 + Delta_X(3)^2)^.5; count = count+1; if count>10 END_LOOP=B1/2; end end%End of While end