gmp2.m

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

%                                gmp2.m
%  Scope:   This MATLAB macro generates second order Gauss-Markov sequence.
%  Usage:   [xp,xv] = gmp2(nstep,w0,beta,csq,deltat,iseed)
%           [xp,xv] = gmp2(nstep,w0,beta,csq,deltat)  when the seed is not reset
%  Description of parameters:
%           nstep  - input, number of steps of the Gauss-Markov process
%           w0     - input, natural frequency in radians/second
%           beta   - input, damping factor which is less than 1.
%           csq    - input, constant  c**2  in meters**2
%           deltat - input, time step in seconds
%           iseed  - input, initial value of the seed (optional); if it is
%                    specified the seed is reset to iseed value
%           xp     - output, computed value of the second order Gauss-Markov
%                    process - first state component  xp, vector with  nstep
%                    elements
%           xv     - output, computed value of the second order Gauss-Markov 
%                    process - second state component  xv, vector with  nstep
%                    elements
%  External Matlab macros used:  genrn
%  Method:
%     Power spectral density function is given by
%           PHI(w) = c**2 / (w**4 + w0**4)
%     where  w  is frequency in radians/sec., w0  is the natural frequency
%     in radians/sec., and  c**2  is a constant in  meters**2.
%     The  process is described by the following second order linear
%     differential equation :
%            ..                    .                    
%            xp  + 2 * beta * w0 * xp  + w0**2 * xp = c * W ,
%     where  W  is the white Gaussian noise with unit power spectral
%     density, and  beta  is a damping factor (less than 1).
%  Last update:  01/07/01
%  Copyright (C) 1996-01 by LL Consulting. All Rights Reserved.


function  [xp,xv] = gmp2(nstep,w0,beta,csq,deltat,iseed)

if ( (nargin < 5) | (nargin > 6) )
   disp('Error - GMP2.m  - check the argument list');
   disp('  ');
   return
elseif  (nargin == 6)
   rand('seed',iseed);  
end

clear xp xv
xp = zeros(nstep,1);
xv = zeros(nstep,1);

for k = 1:nstep
   if k == 1
      sigp = sqrt(csq / (4.0 * beta * w0 * w0 * w0));
      sigv = w0 * sigp;
      w1 = (sigv / sigp) * sqrt(1.0 - beta * beta);
      temp1 = exp( - beta * w0 * deltat);
      temp2 = cos(w1 * deltat);
      temp3 = sin(w1 * deltat);
      temp4 = (beta * w0 / w1) * temp3;
      phi11 = temp1 * (temp2 + temp4);
      phi12 = (temp1 / w1) * temp3;
      phi21 = - w0 * w0 * phi12;
      phi22 = temp1 * (temp2 - temp4);
      temp1 = exp( - 2.0 * beta * w0 * deltat);
      temp2 = cos(2.0 * w1 * deltat);
      temp3 = beta * beta * temp2;
      temp4 = beta * (w1 / w0) * sin(2.0 * w1 * deltat);
      temp5 = csq / (4.0 * beta * w0);
      temp6 = ((w0 * w0) / (w1 * w1)) * temp1;
      q11 = (temp5 / (w0 * w0)) * (1.0 - temp6 * (1.0 - temp3 + temp4));
      q12 = (csq / (4.0 * w1 * w1)) * (temp1 * (1.0 - temp2));
      q21 = q12;
      q22 = temp5 * (1.0 - temp6 * (1.0 - temp3 - temp4));
      u11 = sqrt(q11 - q12 * q12 / q22);
      u12 = q12 / sqrt(q22);
      u22 = sqrt(q22);
      xptemp = genrn(1,0.,sigp);
      xvtemp = genrn(1,0.,sigv);
   end
   w1 = genrn(1,0.,1.0);
   w2 = genrn(1,0.,1.0);
   xp(k) = phi11 * xptemp + phi12 * xvtemp + u11 * w1 + u12 * w2;
   xv(k) = phi21 * xptemp + phi22 * xvtemp + u22 * w2;
   xptemp = xp(k);
   xvtemp = xv(k);
end