gusucode.com > 扩展卡尔曼滤波源码程序 > 扩展卡尔曼滤波源码程序/EKF/EKF/GPS_EKF.m
% Example: % Kalman filter for GPS positioning % This file provide an example of using the Extended_KF function with the % the application of GPS navigation. The pseudorange and satellite position % of a GPS receiver at fixed location for a period of 25 seconds is % provided. Least squares and Extended KF are used for this task. % % The following is a brief illustration of the principles of GPS. For more % information see reference [2]. % The Global Positioning System(GPS) is a satellite-based navigation system % that provides a user with proper equipment access to positioning % information. The most commonly used approaches for GPS positioning are % the Iterative Least Square(ILS) and the Kalman filtering(KF) methods. % Both of them is based on the pseudorange equation: % rho = || Xs - X || + b + v % in which Xs and X represent the position of the satellite and % receiver, respectively, and || Xs - X || represents the distance between % them. b represents the clock bias of receiver, and it need to be solved % along with the position of receiver. rho is a measurement given by % receiver for each satellites, and v is the pseudorange measurement noise % modeled as white noise. % There are 4 unknowns: the coordinate of receiver position X and the clock % bias b. The ILS can be used to calculate these unknowns and is % implemented in this example as a comparison. In the KF solution we use % the Extended Kalman filter (EKF) to deal with the nonlinearity of the % pseudorange equation, and a CV model (constant velocity)[1] as the process % model. % References: % 1. R G Brown, P Y C Hwang, "Introduction to random signals and applied % Kalman filtering : with MATLAB exercises and solutions",1996 % 2. Pratap Misra, Per Enge, "Global Positioning System Signals, % Measurements, and Performance(Second Edition)",2006 clear all close all clc load SV_Pos % position of satellites load SV_Rho % pseudorange of satellites T = 1; % positioning interval N = 25;% total number of steps % State vector is as [x Vx y Vy z Vz b d].', i.e. the coordinate (x,y,z), % the clock bias b, and their derivatives. % Set f, see [1] f = @(X) ConstantVelocity(X, T); % Set Q, see [1] Sf = 36;Sg = 0.01;sigma=5; %state transition variance Qb = [Sf*T+Sg*T*T*T/3 Sg*T*T/2; Sg*T*T/2 Sg*T]; Qxyz = sigma^2 * [T^3/3 T^2/2; T^2/2 T]; Q = blkdiag(Qxyz,Qxyz,Qxyz,Qb); % Set initial values of X and P X = zeros(8,1); X([1 3 5]) = [-2.168816181271560e+006 4.386648549091666e+006 4.077161596428751e+006]; %Initial position X([2 4 6]) = [0 0 0]; %Initial velocity X(7,1) = 3.575261153706439e+006; %Initial clock bias X(8,1) = 4.549246345845814e+001; %Initial clock drift P = eye(8)*10; fprintf('GPS positioning using EKF started\n') tic for ii = 1:N % Set g g = @(X) PseudorangeEquation(X, SV_Pos{ii}); % pseudorange equations for each satellites % Set R Rhoerror = 36; % variance of measurement error(pseudorange error) R = eye(size(SV_Pos{ii}, 1)) * Rhoerror; % Set Z Z = SV_Rho{ii}.'; % measurement value [X,P] = Extended_KF(f,g,Q,R,Z,X,P); Pos_KF(:,ii) = X([1 3 5]).'; % positioning using Kalman Filter Pos_LS(:,ii) = Rcv_Pos_Compute(SV_Pos{ii}, SV_Rho{ii}); % positioning using Least Square as a contrast fprintf('KF time point %d in %d ',ii,N) time = toc; remaintime = time * N / ii - time; fprintf('Time elapsed: %f seconds, Time remaining: %f seconds\n',time,remaintime) end % Plot the results. Relative error is used (i.e. just subtract the mean) % since we don't have ground truth. for ii = 1:3 subplot(3,1,ii) plot(1:N, Pos_KF(ii,:) - mean(Pos_KF(ii,:)),'-r') hold on;grid on; plot(1:N, Pos_LS(ii,:) - mean(Pos_KF(ii,:))) legend('EKF','ILS') xlabel('Sampling index') ylabel('Error(meters)') end ha = axes('Position',[0 0 1 1],'Xlim',[0 1],'Ylim',[0 1],'Box','off','Visible','off','Units','normalized', 'clipping' , 'off'); text(0.5, 1,'\bf Relative positioning error in x,y and z directions','HorizontalAlignment','center','VerticalAlignment', 'top');