xkfcov.m

来自「GPS software toolbox for GPS receiver de」· M 代码 · 共 213 行

%                                xkfcov.m
%  Scope:   This MATLAB program performs covariance analysis by using the 
%           conventional or alternate conventional Kalman filter formulation. 
%  Usage:   xkfcov
%  Inputs:  - number of steps to be processed (nstep)
%           - selection of the variant to be used
%           - selection of the input data from a selected input data file
%             or the default input data set.
%             The structure of the input file is as follows:
%             -- first row contains the values for state dimension (n) and 
%                measurement dimension (m)
%             -- next n rows contain the corresponding rows of the transition matrix (phi)
%             -- next n rows contain the corresponding rows of the process noise matrix 
%                (q)
%             -- next m rows contain the corresponding rows of the observation matrix (h)
%             -- next m rows contain the corresponding rows of the measurement noise 
%                matrix (r)
%             -- next n rows contain the corresponding rows of the initial covariance 
%                matrix (depending on the variant used)
%           - index of the state covariance to be saved or/and plotted
%           - name of the output file, if the generated data are saved
%  Outputs: - output file containing the generated data (optional)
%             At each time step the following data are stored:  step number, standard
%             deviation after and before measurement incorporation
%           - plot of the selected state standard deviation versus time sequence
%  External Matlab macros used:  kfcov, kfcova
%  Last update:  10/31/00
%  Copyright (C) 1996-2000 by LL Consulting. All Rights Reserved.

clear 
close all

%  Initialization and the selection of the input data

yes = 'y';
disp('  ');
nstep = input('Specify the number of steps, i.e nstep --> ');
disp('  ');
disp('Specify the variant to be used: ');
disp('Select:   1  for conventional Kalman filter ');
disp('          2  for alternate conventional Kalman filter ');
variant = input('Make the selection  --> ');
disp('  ');
if ( (variant ~= 1) & (variant ~= 2) )
   disp('Error  XKFCOV.m  - check the selection of the method ');
   disp('  ');
   break;
end   
answer = input('Do you want to use the default data? (y/n)[n] --> ','s');
disp('  ');

phi = [];      %zeros(n,n);
q = [];        %zeros(n,n);
h = [];        %zeros(m,n);
r = [];        %zeros(m,m);
pbefore = [];  %zeros(n,n);
pinvbef = [];  %zeros(n,n);
pafter = [];   %zeros(n,n);
if (strcmp(answer,yes) == 1)    % default data set
   n = 1;                       % state dimension 
   m = 1;                       % measurement dimension
   phi = 1;                     % transition matrix
   q = 4;                       % process noise matrix
   h = 0.1;                     % measurement matrix
   r = 1;                       % measurement noise matrix
   if  (variant == 1)
      pbefore = 100;            % covariance matrix before
   else     % variant = 2   
      pinvbef = 0.01;           % inverse of covariance matrix before
   end   
else                            % read input data from the selected file
   fname = input('Specify the name of the input file (with extension) -- > ','s');
   disp('  ');
   finp = fopen(fname);
   eof = 0;
   line = fgetl(finp);
   if (line == -1), eof = 1; break; end
   n = str2num(line(1:10));
   m = str2num(line(11:20));
   for k = 1:n
      line = fgetl(finp);
      if (line == -1), eof = 1; break; end
      kkk = 1;
      for kk = 1:n
         phi(k,kk) = str2num(line(kkk:kkk+9));
         kkk = kkk + 10;
      end
   end
   for k = 1:n
      line = fgetl(finp);
      if (line == -1), eof = 1; break; end
      kkk = 1;
      for kk = 1:n
         q(k,kk) = str2num(line(kkk:kkk+19));
         kkk = kkk + 20;
      end
   end
   for k = 1:m
      line = fgetl(finp);
      if (line == -1), eof = 1; break; end
      kkk = 1;
      for kk = 1:n
         h(k,kk) = str2num(line(kkk:kkk+9));
         kkk = kkk + 10;
      end
   end
   for k = 1:m
      line = fgetl(finp);
      if (line == -1), eof = 1; break; end
      kkk = 1;
      for kk = 1:m
         r(k,kk) = str2num(line(kkk:kkk+9));
         kkk = kkk + 10;
      end
   end
   for k = 1:n
      line = fgetl(finp);
      if (line == -1), eof = 1; break; end
   	kkk = 1;
    	for kk = 1:n
     	  if  (variant == 1)
        	   pbefore(k,kk) = str2num(line(kkk:kkk+9));
     	  else     % variant = 2   
        	   pinvbef(k,kk) = str2num(line(kkk:kkk+9));
      	end   
         kkk = kkk + 10;
      end   
   end
end

nstep1 = nstep + 1;
x = [];    % zeros(nstep,1);
x = 1:nstep;
y1 = [];   % zeros(nstep1,1);
y2 = [];   % zeros(nstep1,1);
if  (variant == 1)
   for k = 1:n
      y2(k) = sqrt(pbefore(k));  
   end   
else     % variant = 2   
   for k = 1:n
      y2(k) = sqrt(inv(pinvbef(k)));  
   end   
end   
xx = [];   % zeros(2*nstep1,1);
yy = [];   % zeros(2*nstep1,1);
yy(1) = y2(1);

%  Select the index of the covariance state variable to be saved or/and plotted

kstate = input('Index of the covariance state to be saved or/and plotted, e.g. 1 --> ');

%  Generation of the covariance matrix

kk = 1;
for k = 1:nstep
   % Update the value of the parameters, i.e. matrices phi, q, h, and r
   % when the process is time dependent
   
   if  (variant == 1)
      [pafter,pbefore] = kfcov(1,n,m,phi,q,h,r,pbefore); 
   else     % variant = 2     
      [pafter,pinvbef] = kfcova(1,n,m,phi,q,h,r,pinvbef);       
   end  
      
   %  Store the selected standard deviations value
   y1(k) = sqrt(pafter(kstate,kstate));
   if  (variant == 1)
      y2(k+1) = sqrt(pbefore(kstate,kstate));
   else     % variant = 2   
      y2(k+1) = sqrt(inv(pinvbef(kstate,kstate)));
   end   
   yy(kk+1) = y1(k);
   yy(kk+2) = y2(k+1);
   xx(kk) = x(k);
   xx(kk+1) = x(k);
   kk = kk + 2;
end   

%  Save the generated data into a specified file if desired

disp('  ');
answer2 = input('Save the generated data? (y/n)[y] --> ','s');
disp('  ');
if  isempty(answer2)
   answer2 = 'y';
end
if strcmp(answer2,yes) == 1
   f2 = input('Specify the output filename --> ','s');
   disp('  ');
   for  k = 1:nstep
      fprintf(f2,'%5d  %f  %f\n',x(k),y1(k),y2(k));
   end
end
   
%  Plot the selected covariance elements
   
aa = ['Selected Standard Deviation versus Time sequence (for state ' num2str(kstate) ')'];
bb = 'Selected Standard Deviation ';
cc = 'Time sequence';
figure(1)
plot(xx(1:2*nstep),yy(1:2*nstep)),...
title(aa), ylabel(bb), xlabel(cc), grid
v = axis;
v(1) = 0.;
v(2) = v(2) + 1;
v(3) = 0.;
v(4) = v(4) + 1;
axis(v);

disp('End of the program  XKFCOV');
disp('  ');