calculate_circum_tools.m

利用奇异值法（SVD）对载入数据进行主成分分析

function S=Calculate_circum_tools27

ErrorNum=27;%误差系数个数

%  载入遥测视速度
load chwx.dat;

%  遥测参数导入
tn=chwx(:,1);                                      % 遥测时间数组
wx=chwx(:,2);                                      % X方向的遥测视速度
wy=chwx(:,3);                                      % Y方向的遥测视速度
wz=chwx(:,4);                                      % Z方向的遥测视速度

format long;

disp('开始计算制导工具误差环境函数矩阵，可能需要1~2分钟，请耐心等待......');

%  常量赋值
%tqt=0;                                            % 单位：秒（s）                 真正发射零点时间
%omega_s=0.7292e-4;                                % 单位：弧度/秒（rad/s）        地球自转角速度常量
g0=9.8005722;                                      % 单位：米/秒^2（m/s2）         引力加速度常量
sec_rad=pi/(180*60*60);                            % 角秒－>弧度
min_rad=pi/(180*60);                               % 角分－>弧度

% 前后差分求解遥测视加速度
dwx=(array_diff(tn,wx))';                      % x方向的遥测视加速度
dwy=(array_diff(tn,wy))';                      % y方向的遥测视加速度
dwz=(array_diff(tn,wz))';                      % z方向的遥测视加速度

n=length(tn);                                      % 数组长度，即样点个数

%  求所有时刻的不完全视加速度环境函数矩阵Sa_t
%  不完全是指不考虑与遥外测视速度常值误差相关的环境函数矩阵
ErrorNum1=ErrorNum-3;
Sa_t=zeros(3,ErrorNum1,n);
for i=1:n
    Sw=[   0     -dwz(i)     dwy(i)
         dwz(i)     0       -dwx(i)
        -dwy(i)   dwx(i)       0   ];
    
    S1=sec_rad*[tn(i),wz(i)/g0,wx(i)/g0,wy(i)/g0];
    S2=sec_rad*[tn(i),wz(i)/g0,wy(i)/g0,wx(i)/g0];
    S3=sec_rad*[tn(i),wy(i)/g0,wz(i)/g0,wx(i)/g0];
    
    S4=[1.0e-4*g0,1.0e-5*g0*dwx(i),min_rad*dwy(i),-min_rad*dwz(i)];
    S5=[1.0e-4*g0,1.0e-5*g0*dwy(i),-min_rad*dwx(i),min_rad*dwz(i)];
    S6=[1.0e-4*g0,1.0e-5*g0*dwz(i),min_rad*dwx(i),-min_rad*dwy(i)];
    
    Sa1=Sw*[S1,0,0,0,0,0,0,0,0;0,0,0,0,S2,0,0,0,0;0,0,0,0,0,0,0,0,S3];
    Sa2=[S4,0,0,0,0,0,0,0,0;0,0,0,0,S5,0,0,0,0;0,0,0,0,0,0,0,0,S6];
    
    Sa_t(:,:,i)=[Sa1,Sa2];
end

%  求所有时刻的不完全视速度环境函数矩阵Sv_t1和不完全视位置环境函数矩阵Sr_t1
Sv_t1=zeros(3,ErrorNum1,n);
Sr_t1=zeros(3,ErrorNum1,n);
for k=1:3
    for j=1:ErrorNum1
        aa=squeeze(Sa_t(k,j,:));           % 获得视加速度环境函数矩阵Sa_t的各元素（折合成关于采样时间的数组）

        bb=trapezia_intergral(tn,aa);      % 梯形积分求视速度环境函数矩阵各元素（是关于采样时间的数组）
        cc=trapezia_intergral(tn,bb);      % 梯形积分求视位置环境函数矩阵各元素（是关于采样时间的数组）

        Sv_t1(k,j,:)=bb;
        Sr_t1(k,j,:)=cc;  
    end
end

%  求与遥外测视速度常值误差相关的环境函数矩阵Sv_t0和Sv_t0
for p=1:n
    Sv_t0(1:3,1:3,p)=[ 1.0    0.0    0.0 
                       0.0    1.0    0.0 
                       0.0    0.0    1.0 ];
    Sr_t0(1:3,1:3,p)=[ tn(p)  0.0    0.0       
                       0.0    tn(p)  0.0      
                       0.0    0.0    tn(p)];
end

%  求所有时刻的环境函数矩阵S_t
S_t=zeros(6,ErrorNum,n);
for m=1:n
    S_t(:,:,m)=[  Sv_t1(:,:,m)     Sv_t0(:,:,m)         % 组成t时刻的环境函数矩阵
                  Sr_t1(:,:,m)     Sr_t0(:,:,m)  ];      
end

% % 选取整秒时刻遥外差
% len1=length(find(tn<16));
% len2=length(find(tn<=60));
% len3=length(find(tn<73));
% len4=length(find(tn<=128));
% len5=length(find(tn<155));
% len6=length(find(tn<=281));
% 
% interval=tn(2)-tn(1);                             % 遥外测数据采样时间间隔
% fre=1.0/interval;                                 % 遥外测数据采样时间频率
% 
% N=(60-15)+(128-73+1)+(281-155+1);
% S_d=zeros(6,ErrorNum,N); 
% 
% %16至60秒数据
% S_d(:,:,1:(len2-len1-1)/fre+1)=S_t(:,:,len1+1:fre:len2);
% 
% %73秒至128秒数据
% b=(len2-len1-1)/fre+1;
% S_d(:,:,b+1:b+(len4-len3-1)/fre+1)=S_t(:,:,len3+1:fre:len4);
% 
% %%155秒至281数据
% b=b+(len4-len3-1)/fre+1;
% S_d(:,:,b+1:b+(len6-len5-1)/fre+1)=S_t(:,:,len5+1:fre:len6);

% 将环境函数矩阵写成(6*N)*27的二维数组形式
len=ceil(length(tn)/20);
S=zeros(6*len,ErrorNum);
for i=1:len
    inte=20*(i-1)+1;
    S((6*i-5):(6*i),:)=S_t(:,:,inte);
end

disp('制导工具误差环境函数矩阵计算完毕！');


%-----------------------------------------------------------------------------------------------------------------------
%-----------------------------------------------------------------------------------------------------------------------


function c=spline_diff(a,b)
%  利用样条函数计算微分
%  c=spline_diff(a,b)
%  a--x1,x2……序列(向量)
%  b--f(x1),f(x2)……序列（向量）
%  c--样条微分值

pp=spline(a,b);                                    % 根据采样点数据(a,b)，获得逐段多项式样条函数数据pp
derivative_pp=fnder(pp);                           % 求样条函数pp的数值导函数derivative_pp
c=ppval(derivative_pp,a);                          % 根据导函数derivative_pp，计算tn向量对应的微分向量

%-----------------------------------------------------------------------------------------------------------------------
%-----------------------------------------------------------------------------------------------------------------------

function c=array_diff(a,b)
%   数组的差分求导函数
%   c=array_diff(a,b)
%   a--x1,x2……序列(向量)
%   b--f(x1),f(x2)……序列（向量）
%   c--(f(x2)-f(x1))/(x2-x1)……导数值
                                        
kk=length(a);                                           % 数组的长度

for ii=1:(kk-1);
    c(ii)=(b(ii+1)-b(ii))/(a(ii+1)-a(ii));              % 差分求导
end; 

%最后一个时刻的导数使用三次样条插值求函数值再差分求导
a(ii+2)=a(ii+1)+a(2)-a(1);
b(ii+2)=spline(a((kk-20):kk),b((kk-20):kk),a(ii+2));
c(ii+1)=(b(ii+2)-b(ii+1))/(a(ii+2)-a(ii+1));


%-----------------------------------------------------------------------------------------------------------------------
%-----------------------------------------------------------------------------------------------------------------------


function y=spline_intergral(tn,x)
%  利用样条函数进行数值积分
%  y=spline_intergral(tn,x)
%  tn--时间序列
%  x--被积数组
%  y--积分返回数组

pp=spline(tn,x);                                   % 根据采样点数据(tn,x)，获得逐段多项式样条函数数据pp
integral_pp=fnint(pp);                             % 求样条函数pp的数值不定积分integral_pp
y=ppval(integral_pp,tn);                           % 根据样条函数integral_pp，计算tn向量对应的积分值向量


%-----------------------------------------------------------------------------------------------------------------------
%-----------------------------------------------------------------------------------------------------------------------


function yy=trapezia_intergral(tn,xx)
%  数组的梯形积分程序
%  yy=trapezia_intergral(tn,xx)
%  s=1/2*(x2-x1)*[f(x2)+f(x1)]
%  tn--x1,x2……序列
%  xx--f(x1),f(x2)……序列
%  yy--积分值序列

n=length(tn);                                      % 数组长度

%  梯形积分
s(1)=0.0;
yy(1)=0.0;
for i=2:n  
    s(i)=1.0/2.0*(xx(i)+xx(i-1))*(tn(i)-tn(i-1));
    yy(i)=yy(i-1)+s(i);
end