gentherr.m

惯性导航系统传递对准系统模型程序

function dtherr = gentherr(deltath,time,errparam,dthseed)

if nargin<4,randn('seed',sum(100*clock)),else,randn('seed',dthseed);end
if nargin<3,error('insufficient number of input arguments'),end

tdint_prof = diff(time);

[m,n] = size(tdint_prof); 
if m<n, 
   tdvec=tdint_prof'; 
   K = n;
else, 
   tdvec=tdint_prof; 
   K = m;
end

dph2rps = (pi/180)/3600;   % conversion constant from deg/hr to rad/sec

dprh2rprs = (pi/180)/sqrt(3600);  % conversion factor going from
%                                 % degrees-per-root-hour to
%                                 % radians-per-root-second

xbias = tdvec*errparam(1,1)*dph2rps;
ybias = tdvec*errparam(1,2)*dph2rps;
zbias = tdvec*errparam(1,3)*dph2rps;
xsferr = deltath(:,1)*errparam(2,1)*0.01;
ysferr = deltath(:,2)*errparam(2,2)*0.01;
zsferr = deltath(:,3)*errparam(2,3)*0.01;
xnoise = ( sqrt(tdvec).*randn(K,1) )*errparam(3,1)*dprh2rprs;
ynoise = ( sqrt(tdvec).*randn(K,1) )*errparam(3,2)*dprh2rprs;
znoise = ( sqrt(tdvec).*randn(K,1) )*errparam(3,3)*dprh2rprs;

dtherr = [xbias+xsferr+xnoise ybias+ysferr+ynoise zbias+zsferr+znoise];