xmudm.m

GPS TOOLBOX包含以下内容： 1、GPS相关常量和转换因子； 2、角度变换； 3、坐标系转换： &#61656 点变换； &#61656 矩阵变换； &#61656 向量变换

%                             xmudm.m
%  Scope:   This MATLAB program tests the macros: mudm, mudm1 and mreast . 
%           Implementation of the discrete Kalman filter measurement updating
%           using Bierman's  U-D  measurement update algorithm. Only a scalar
%           measurement is processed, and the  U-D  covariance factors and the     
%           state estimate (optional) are updated.               
%           The macro  mudm  has the scalar measurement as an input, while the
%           macro  mudm1  has the measurement residual as an input.                            
%           The macro  mreast  executes the measurement reasonableness test
%           for a specific measurement.      
%  Usage:   xmudm
%  Inputs:  - name of the output file if selected (the default is the screen)
%           - dimension of state vector  n, n >= 2
%           - elements of the U-D factors, ud, real array of length  
%             (n+1)*(n+2)/2. The first  n*(n+1)/2  elements store the 
%             input/output U-D  factors; the  D  elements are stored on 
%             diagonal. The last  n+1  elements store the input/output state
%             vector  x  and the scalar value  z-h**(transpose)*x(a priori) 
%             in  ud((n+1)*(n+2)/2).
%             If  index = 0  then the state  x  (and  z) need not be included
%             and the dimension of the array should be  n*(n+1)/2 .
%           - measurement variance, r
%           - vector of measurement coefficients (first  n  elements) and
%             h(n+1) = z  if  index = 0 . If  index = 0  the dimension of the
%             array  h  should be  n .
%           - index of updating type, index
%             = 0    then only the  U-D  factors are updated (the state is 
%                    not updated)
%             ~= 0   then  the  U-D  factors and state are updated
%           - reasonableness test threshold
%  Outputs: - input/output data stored into the specified output file or
%             displayed on screen
%             The output data are as follows:
%             - for the computation executed by using macro mreast
%               - a pass/fail test message 
%             - for the computation executed by using macro mudm
%               - UD matrix (output)
%               - state vector (output), optional 
%             - for the computation executed by using macro mudm1
%               - UD matrix (output)
%               - state vector (output), optional 
%  External Matlab macros used:  mreast, mudc2f, mudm, mudm1                     
%  Last update:  07/22/00
%  Copyright (C) 1996-00 by LL Consulting. All Rights Reserved.
                                                                       
clear

%  Select the input data

yes = 'y';
disp('  ');
answer = input('Do you want to save the results? (y/n)[n] --> ','s');
disp('  ');
if (strcmp(answer,yes) == 1)
   f2 = input('Specify the output filename  -->  ','s');
   disp('  ');
else
   f2 = 1;      %  output to the screen
end
answer = input('Do you want to use the default data? (y/n)[n] --> ','s');
disp('  ');
if (strcmp(answer,yes) == 1)
   n = 2;                             %  state vector dimension
   index = 1;                         %  updating type: 0 = only U-D factors,
                                      %  ~0 = U-D factors and state 
   np1 = n + 1;
   nn1 = n * (n+1) / 2;
   nn2 = nn1 + n + 1;
   ud = zeros(1,nn2);
   h = zeros(1,np1);
   ud = [ 2. 0.5 8.];                 %  ud  matrix  (input)                
   xstate = [ 1. 2.];                 %  state (input)
   z = 0.5;                           %  measurement 
   r = 1.;                            %  measurement variance           
   h = [1. 1.];                       %  observation matrix
   test = 9.;                         %  test threshold corresponds to 3 
                                      %  sigma verification
else
   n = input('Enter state vector dimension, n --> ');
   disp('  ');
   index = input('Enter index of updating, 0 = only U-D, ~0 = else, --> ');
   disp('  ');
   np1 = n + 1;
   nn1 = n * (n+1) / 2;
   nn2 = nn1 + n + 1;
   ud = zeros(1,nn2);
   h = zeros(1,np1);
   udtemp = zeros(1,nn2);
   for k = 1:nn1
      ud(k) = input('Enter the elements of the matrix U-D --> ');
      disp('  ');
   end
   if index ~= 0
      for k = 1:n
         xstate(k) = input('Enter the elements of the state vector, x --> ');
         disp('  ');
      end
   end
   if index ~= 0
      z = input('Enter the measurement value, z --> ');
      disp('  ');
   end
   r = input('Enter measurement variance, r --> ');
   disp('  ');
   for k = 1:n
      h(k) = input('Enter the elements of the measurement vector, h --> ');
      disp('  ');
   end
   test = input('Enter reasonableness test threshold --> ');
   disp('  ');
end

%  Initialization

udtemp = ud;
if index ~= 0
   zres = z;
   for k = 1:n
      zres = zres - h(k) * xstate(k);     %   measurement residual
   end 
end

%  Save or display the input data

fprintf(f2,'***************************************************************\n\n');
fprintf(f2,'*****   Input data   *****  \n\n');
fprintf(f2,'     UD  matrix (input) - compact form  \n');
for k = 1:nn1
   if rem(k,7) == 0
      fprintf(f2,'\n'); 
   end
   fprintf(f2,'%14.4e  ',ud(k)); 
end
[ufull,dfull] = mudc2f(n,ud);
fprintf(f2,'\n\n     U  matrix (output) - unit upper triangular matrix \n');
for k = 1:n
    for kk = 1:n
        fprintf(f2,'%14.4e  ',ufull(k,kk));
    end
    fprintf(f2,'\n');
 end   
fprintf(f2,'\n     D  matrix (output) - diagonal matrix \n');
for k = 1:n
    for kk = 1:n
        fprintf(f2,'%14.4e  ',dfull(k,kk));
    end
    fprintf(f2,'\n');
end   
if index ~= 0
   fprintf(f2,'\n     State  (input) \n');
   for k = 1:n
      if rem(k,7) == 0
         fprintf(f2,'\n'); 
      end
      fprintf(f2,'%14.4e  ',xstate(k) );  
   end
end
fprintf(f2,'\n\n     Variance = %8.4e \n\n',r);
fprintf(f2,'     Measurement vector \n');
for k = 1:n
   if rem(k,7) == 0
      fprintf(f2,'\n'); 
   end
   fprintf(f2,'%14.4e  ',h(k) );          
end
fprintf(f2,'\n\n     Reasonableness test threshold = %8.4e \n\n',test);

%  Use of the macro  mreast  (reasonableness test)

if index ~= 0
   alpha = mreast(n,ud,h,r,zres,test);
   fprintf(f2,'*****   Results from macro  mreast (reasonableness test)  *****  \n\n');
   if alpha >= 0.
       fprintf(f2,'     The measurement passed the reasonableness test\n');
   else
       fprintf(f2,'     The measurement failed the reasonableness test\n');
   end 
   disp('  ');
end

if index ~= 0
   for k = 1:n
      ud(nn1+k) = xstate(k);
      udtemp(nn1+k) = xstate(k);
   end 
end
                        
%  Use of the macro  mudm  (measurement as input)

if index ~= 0
   h(n+1) = z;
end

[ud,f,g,alpha] = mudm(n,ud,r,h,index);

fprintf(f2,'*****   Results from macro  mudm  (measurement as input) *****\n\n');
fprintf(f2,'     UD  matrix (output) - compact form\n');
for k = 1:nn1
   if rem(k,7) == 0
      fprintf(f2,'\n'); 
   end
   fprintf(f2,'%14.4e  ',ud(k)); 
end
[ufull,dfull] = mudc2f(n,ud);
fprintf(f2,'\n\n     U  matrix (output) - unit upper triangular matrix \n');
for k = 1:n
    for kk = 1:n
        fprintf(f2,'%14.4e  ',ufull(k,kk));
    end
    fprintf(f2,'\n');
 end   
fprintf(f2,'\n     D  matrix (output) - diagonal matrix \n');
for k = 1:n
    for kk = 1:n
        fprintf(f2,'%14.4e  ',dfull(k,kk));
    end
    fprintf(f2,'\n');
end   
if index ~= 0
   fprintf(f2,'\n     State  (output) \n');
   for k = 1:n
      if rem(k,7) == 0
         fprintf(f2,'\n'); 
      end
      fprintf(f2,'%14.4e  ',ud(nn1+k) );   %   state  (output)
   end
   fprintf(f2,'\n');
end
disp('  ');

%  Use of the macro  mudm1  (measurement residual as input)

if index ~= 0
   udtemp(nn2) = zres;
end

[udtemp,f,g,alpha] = mudm1(n,udtemp,r,h,index);

fprintf(f2,'*****   Results from macro  mudm1 (measurement residual as input)  *****\n\n');
fprintf(f2,'     UD  matrix (output) - comnpact form \n');
for k = 1:nn1
   if rem(k,7) == 0
      fprintf(f2,'\n'); 
   end
   fprintf(f2,'%14.4e  ',udtemp(k) ); 
end
[ufull,dfull] = mudc2f(n,udtemp);
fprintf(f2,'\n\n     U  matrix (output) - unit upper triangular matrix \n');
for k = 1:n
    for kk = 1:n
        fprintf(f2,'%14.4e  ',ufull(k,kk));
    end
    fprintf(f2,'\n');
 end   
fprintf(f2,'\n     D  matrix (output) - diagonal matrix \n');
for k = 1:n
    for kk = 1:n
        fprintf(f2,'%14.4e  ',dfull(k,kk));
    end
    fprintf(f2,'\n');
end   
if index ~= 0
   fprintf(f2,'\n     State  (output) \n');
   for k = 1:n
      if rem(k,7) == 0
         fprintf(f2,'\n'); 
      end
      fprintf(f2,'%14.4e  ',udtemp(nn1+k) );     %   state  (output)
   end
end
fprintf(f2,'\n\n***************************************************************\n');

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