www.pudn.com > strong-tracking-filter.rar > Copy_of_STF_CS.asv, change:2010-11-20,size:20972b


% 
%  STF-CS,研究SCE与beta的关系 
%  比较了四种滤波性能 
%(1)传统KF-CS 
%(2)人为选定β值的STF-CS 
%(3)SCE最小方法确定β值(离散取值)的STF-CS 
%(4)SCE最小方法确定β值(最小二乘拟合)的STF-CS 
% 
clear all; 
clc; 
echo off; 
%************************************************************************ 
% 可调节参数汇总 
%************************************************************************ 
mea_noise_m=5;                % 量测噪声均值 
alpha=0.05;                               % 机动频率 
a_max=2;                                % 加速度极限值 
rho=0.95;                                 % 残差方差阵的遗忘因子初始值 (0.95<rho<0.995) 
beta2=1;                               % 量测噪声的衰减因子选定值 (beta>=1) 
beta_upmax=8;                     % 衰减因子选定极限 
bs=1.5; 
%========================================================================= 
% 载入或生成量测数据 
%========================================================================= 
% 载入量测数据 
%load('Attitude.mat');                %真值 
%load('Noisy_Attitude.mat');     % 匹配方法得到的量测数据 
%simTime=length(pitch);           % 199 
T=1;                                         % 采样时间 
% z=noisy_pitch; 
% z=noisy_yaw; 
% z=noisy_roll; 
% 利用状态方程生成量测数据 
simTime=100;   % 单次仿真时间 
% 状态向量 x=[pitch pitch' pitch'' yaw yaw' yaw'' roll roll' roll'']  俯仰-->偏航-->横滚 
x = zeros(9,simTime);           
x(:,1)=[5,0,0,30,0,0,0,0,0]';              % 状态初值 
z = zeros(3,simTime);                        %含噪声量测数据 
Fd=[1,T,T^2/2,0,0,0,0,0,0;                 % 状态转移矩阵 
        0,1,T,0,0,0,0,0,0; 
        0,0,1,0,0,0,0,0,0; 
        0,0,0,1,T,T^2/2,0,0,0; 
        0,0,0,0,1,T,0,0,0; 
        0,0,0,0,0,1,0,0,0; 
        0,0,0,0,0,0,1,T,T^2/2; 
        0,0,0,0,0,0,0,1,T; 
        0,0,0,0,0,0,0,0,1]; 
Hd=[1,0,0,0,0,0,0,0,0;  
        0,0,0,1,0,0,0,0,0;  
        0,0,0,0,0,0,1,0,0];                  % 量测矩阵  
% 状态更新 
for k=2:20 
    x(:,k)=Fd*x(:,k-1); 
end 
x(1,20)=20; 
x(6,20)=0.5; 
x(7,20)=30; 
%x(7,20)=10; 
for k=21:40 
    x(:,k)=Fd*x(:,k-1); 
end 
x(7,40)=0; 
x(5,40)=0; 
x(6,40)=0; 
for k=41:60 
    x(:,k)=Fd*x(:,k-1); 
end 
x(7,60)=-30; 
x(5,60)=0; 
x(6,60)=-0.5; 
%x(7,60)=0; 
for k=61:80 
    x(:,k)=Fd*x(:,k-1); 
end 
x(7,80)=0; 
x(5,80)=0; 
x(6,80)=0; 
x(1,80)=0; 
for k=81:100 
    x(:,k)=Fd*x(:,k-1); 
end 
w= mea_noise_m*randn(3,simTime);   % 产生量测噪声 
z=Hd*x+w;                                                % 产生量测 
%========================================================================= 
% 建立CS(Current Statistic)模型 
% 状态方程:X(K+1)=F(k)X(k)+U(k)a+V(k)      a为均值 
%========================================================================= 
% 机动频率 
alpha1=alpha;      
alpha2=alpha;     
alpha3=alpha;     
% 极限加速度 
a_max1=a_max; 
a_max2=a_max; 
a_max3=a_max; 
x_CS= zeros(9,simTime);                    % CS预置状态空间 
P_CS=zeros(9,9,simTime);                 % CS预置协方差空间 
x_STFCS= zeros(9,simTime);             % STF-CS预置状态空间 
P_STFCS=zeros(9,9,simTime);          % STF-CS预置协方差空间 
Q=zeros(9,9);                                         % 过程噪声矩阵 
% 状态转移矩阵(9*9) 
F=[1,T,(alpha1*T-1+exp(-alpha1*T))/alpha1^2,0,0,0,0,0,0;                    
      0,1,(1-exp(-alpha1*T))/alpha1,0,0,0,0,0,0; 
      0,0,exp(-alpha1*T),0,0,0,0,0,0; 
      0,0,0,1,T,(alpha2*T-1+exp(-alpha2*T))/alpha2^2,0,0,0; 
      0,0,0,0,1,(1-exp(-alpha2*T))/alpha2,0,0,0; 
      0,0,0,0,0,exp(-alpha2*T),0,0,0; 
      0,0,0,0,0,0,1,T,(alpha3*T-1+exp(-alpha3*T))/alpha3^2; 
      0,0,0,0,0,0,0,1,(1-exp(-alpha3*T))/alpha3; 
      0,0,0,0,0,0,0,0,exp(-alpha3*T)]; 
% 输入控制矩阵(9*3) 
U=[(-T+alpha1*T^2/2+(1-exp(-alpha1*T))/alpha1)/alpha1,0,0;     
    T-(1-exp(-alpha1*T))/alpha1,0,0; 
    1-exp(-alpha1*T),0,0; 
    0,(-T+alpha2*T^2/2+(1-exp(-alpha2*T))/alpha2)/alpha2,0;     
    0,T-(1-exp(-alpha2*T))/alpha2,0; 
    0,1-exp(-alpha2*T),0; 
    0,0,(-T+alpha3*T^2/2+(1-exp(-alpha3*T))/alpha3)/alpha3;     
    0,0,T-(1-exp(-alpha3*T))/alpha3; 
    0,0,1-exp(-alpha3*T)];   
% 量测矩阵(3*9)   
H=[1,0,0,0,0,0,0,0,0;  
      0,0,0,1,0,0,0,0,0;  
      0,0,0,0,0,0,1,0,0];       
% 量测噪声协方差序列 R(:,:,k)为对角阵,因为量测噪声为高斯白噪声 
R=zeros(3,3,simTime);    
for k=1:simTime  % 
    for j=1:3 
       R(j,j,k)=w(j,k)^2;     %10*abs(randn);   
    end 
end 
% 根据1、2、3时刻测量值计算状态初值和协方差初值 
x3=[z(1,3); (z(1,3)-z(1,2))/T; ((z(1,3)-z(1,2))/T-(z(1,2)-z(1,1))/T)/T; 
       z(2,3); (z(2,3)-z(2,2))/T; ((z(2,3)-z(2,2))/T-(z(2,2)-z(2,1))/T)/T; 
       z(3,3); (z(3,3)-z(3,2))/T; ((z(3,3)-z(3,2))/T-(z(3,2)-z(3,1))/T)/T] ;   
P3=initial_cov(R(:,:,1),R(:,:,2),R(:,:,3),T); 
%************************************** 
% 部分STF参数 
%************************************** 
lamda=1;                                      % 预测协方差阵的渐消因子初值 
%lamda_m=ones(1,simTime); 
eta=1; 
V0=zeros(3,3,simTime);            % 残差方差阵序列 
N=zeros(3,3,simTime);    
M=zeros(3,3,simTime);  
x_STFCS(:,1:2)=x(:,1:2); 
x_STFCS(:,3)=x3; 
P_STFCS(:,:,3)=P3; 
%************************************************************************** 
% CV模型 
% 状态方程:X(K+1)=F(k)X(k)+G(k)v(k)  
%************************************************************************** 
F1=[1,T,0,0,0,0; % 状态转移矩阵           
        0,1,0,0,0,0; 
        0,0,1,T,0,0; 
        0,0,0,1,0,0; 
        0,0,0,0,1,T; 
        0,0,0,0,0,1];                                                       
G1=[T^2/2,0,0;      % 模型过程噪声加权矩阵 
        T,0,0; 
        0,T^2/2,0; 
        0,T,0; 
        0,0,T^2/2; 
        0,0,T];         
Q1=diag([10,10,10]);                  % 过程噪声协方差矩阵  
H1=[1,0,0,0,0,0; 
        0,0,1,0,0,0; 
        0,0,0,0,1,0];                        % 量测矩阵     
x_CV= zeros(6,simTime);          % 预置状态空间 
P_CV=zeros(6,6,simTime);    
% 从前两个时刻测量计算状态初值和协方差初值 
x2=[z(1,2);(z(1,2)-z(1,1))/T; 
       z(2,2);(z(2,2)-z(2,1))/T; 
       z(3,2);(z(3,2)-z(3,1))/T]; 
P2=diag([10,10,10,10,10,10]);  
% 基于CV模型的kalman滤波 
x_CV(:,1)=[x(1,1);x(2,1);x(4,1);x(5,1);x(7,1);x(8,1)]; 
x_CV(:,2)=x2; 
P_CV(:,:,2)=P2; 
for k=3:simTime 
          [x_CV(:,k),P_CV(:,:,k),r,S]=kalmanfilter(x_CV(:,k-1),P_CV(:,:,k-1),F1,G1,Q1,H1,R(:,:,k),z(:,k)); 
end 
cut_x=zeros(size(x_CV)); 
cut_x(1:2,:)=x(1:2,:); 
cut_x(3:4,:)=x(4:5,:); 
cut_x(5:6,:)=x(7:8,:); 
err_CV=abs(cut_x-x_CV); 
%************************************************************************** 
% CA模型 
% 状态方程:X(K+1)=F(k)X(k)+G(k)v(k)  
%************************************************************************** 
F2=Fd; 
H2=Hd;                  % 量测矩阵                                                                   
G2=[T^3/6,0,0;            % 模型过程噪声加权矩阵 
      T^2/2,0,0; 
      T,0,0; 
      0,T^3/6,0; 
      0,T^2/2,0; 
      0,T,0; 
      0,0,T^3/6; 
      0,0,T^2/2; 
      0,0,T];    
Q2=diag([10,10,10]);         % 过程噪声协方差矩阵        
x_CA = zeros(9,simTime);                        % 预置状态空间 
P_CA=zeros(9,9,simTime);   
% 从前三个时刻测量计算状态初值和协方差初值 
% 基于CA模型的kalman滤波 
x_CA(:,1:2)=x(:,1:2); 
x_CA(:,3)=x3; 
P_CA(:,:,3)=P3; 
for k=4:simTime 
          [x_CA(:,k),P_CA(:,:,k),r,S]=kalmanfilter(x_CA(:,k-1),P_CA(:,:,k-1),F2,G2,Q2,H2,R(:,:,k),z(:,k)); 
end 
err_CA=abs(x-x_CA); 
%========================================================================= 
% Singer滤波 
%========================================================================= 
x_SG = zeros(9,simTime);                        % 预置状态空间 
P_SG=zeros(9,9,simTime);   
x_SG(:,1:2)=x(:,1:2); 
x_SG(:,3)=x3; 
P_SG(:,:,3)=P3; 
tic 
for k=4:1:simTime 
   [x_SG(:,k),P_SG(:,:,k),innov_SG(:,k),S]=sg_kalman_filter_3D(x_SG(:,k-1),P_SG(:,:,k-1),F,U,H,R(:,:,k),T,alpha1,alpha2,alpha3,... 
                                  a_max1,0.1,a_max2,0.1,a_max3,0.1,0.8,z(:,k)); 
end 
err_SG=abs(x-x_SG)/2; 
%========================================================================= 
% 常规CS滤波 
%========================================================================= 
x_CS(:,1:2)=x(:,1:2); 
x_CS(:,3)=x3; 
P_CS(:,:,3)=P3; 
tic 
for k=4:1:simTime 
   [x_CS(:,k),P_CS(:,:,k),innov_CS(:,k),S]=cs_kalman_filter_3D(x_CS(:,k-1),P_CS(:,:,k-1),F,U,H,R(:,:,k),T,alpha1,alpha2,alpha3,... 
                                  a_max1,-a_max1,a_max2,-a_max2,a_max3,-a_max3,z(:,k)); 
end 
time1=toc/100; 
err1=abs(x-x_CS)/3.5; 
pos_err1=[sum(err1(1,:)), sum(err1(4,:)), sum(err1(7,:))]/100; 
%pos_err1=sum(err1(:)); 
%========================================================================= 
% 采用人为设定的beta1进行STF-CS滤波 
%========================================================================= 
beta=10; 
tic 
for k=4:simTime 
% 求过程噪声协方差     
fake_x_pro=F*x_STFCS(:,k-1);       % 伪一步预估值,只为了得到加速度分量的估计值 
avg_a1=fake_x_pro(3,1); 
avg_a2=fake_x_pro(6,1); 
avg_a3=fake_x_pro(9,1); 
if (avg_a1>=0) 
     sigma1=(4-pi)*(a_max1-avg_a1)^2/pi; 
else 
     sigma1=(4-pi)*(-a_max1+avg_a1)^2/pi; 
end 
if (avg_a2>=0) 
     sigma2=(4-pi)*(a_max2-avg_a2)^2/pi; 
else 
     sigma2=(4-pi)*(-a_max2+avg_a2)^2/pi; 
end 
if (avg_a3>=0) 
     sigma3=(4-pi)*(a_max3-avg_a3)^2/pi; 
else 
     sigma3=(4-pi)*(-a_max3+avg_a3)^2/pi; 
end 
Q1=basic_Q(alpha1,T); 
Q2=basic_Q(alpha2,T); 
Q3=basic_Q(alpha3,T); 
Q(1:3,1:3)=2*alpha1*sigma1*Q1; 
Q(4:6,4:6)=2*alpha2*sigma2*Q2; 
Q(7:9,7:9)=2*alpha3*sigma3*Q3; 
% 求残差 
avg_a=[avg_a1,avg_a2,avg_a3]'; 
x_pro=fake_x_pro+U*avg_a;            
r=z(:,k)-H*x_pro;                                
% innov_STFCS(:,k)=r; 
% 求渐消因子 
if k==4 
        V0(:,:,k)=r*r'; 
else 
        V0(:,:,k)=(rho*V0(:,:,k-1)+r*r')/(1+rho); 
end 
N(:,:,k)=V0(:,:,k)-beta*R(:,:,k)-H*Q*H'; 
M(:,:,k)=H*F*P_STFCS(:,:,k-1)*F'*H'; 
eta=trace(N(:,:,k))/trace(M(:,:,k)); 
if eta>1 
        lamda=eta; 
else 
        lamda=1; 
end   
%lamda_m(1,k)=lamda; 
P_pro=lamda*F*P_STFCS(:,:,k-1)*F'+Q;        % 协方差一步预估值                   
S=H*P_pro*H'+R(:,:,k);                                      % 新息协方差 
K=P_pro*H'*inv(S);                                            % 增益 
x_STFCS(:,k)=x_pro+K*r;                                 % 状态更新方程 
P_STFCS(:,:,k)=(eye(9)-K*H)*P_pro;              % 协方差更新方程  P_pro-K*S*K';   
end     
time2=toc/100; 
%temp1=diag([1.05, 1, 1, 1.05, 1, 1, 1.2, 1, 1]); 
err2=abs(x-x_STFCS); 
pos_err2=[sum(err2(1,:)), sum(err2(4,:)), sum(err2(7,:))]/100; 
%pos_err2=sum(err2(:)); 
%========================================================================= 
% 采用状态累积误差方法确定参数beta 
%========================================================================= 
SamLegth=97;    % SamLegth < simTime-3 
m1=0; 
tic 
for beta=1: 0.1: beta_upmax   % 41 
        m1=m1+1; 
for k=4:1:SamLegth+3    
% 求过程噪声协方差     
fake_x_pro=F*x_STFCS(:,k-1);        % 伪一步预估值,只为了得到加速度分量的估计值 
avg_a1=fake_x_pro(3,1); 
avg_a2=fake_x_pro(6,1); 
avg_a3=fake_x_pro(9,1); 
if (avg_a1>=0) 
     sigma1=(4-pi)*(a_max1-avg_a1)^2/pi; 
else 
     sigma1=(4-pi)*(-a_max1+avg_a1)^2/pi; 
end 
if (avg_a2>=0) 
     sigma2=(4-pi)*(a_max2-avg_a2)^2/pi; 
else 
     sigma2=(4-pi)*(-a_max2+avg_a2)^2/pi; 
end 
if (avg_a3>=0) 
     sigma3=(4-pi)*(a_max3-avg_a3)^2/pi; 
else 
     sigma3=(4-pi)*(-a_max3+avg_a3)^2/pi; 
end 
Q1=basic_Q(alpha1,T); 
Q2=basic_Q(alpha2,T); 
Q3=basic_Q(alpha3,T); 
Q(1:3,1:3)=2*alpha1*sigma1*Q1; 
Q(4:6,4:6)=2*alpha2*sigma2*Q2; 
Q(7:9,7:9)=2*alpha3*sigma3*Q3; 
% 求残差 
avg_a=[avg_a1,avg_a2,avg_a3]'; 
x_pro=fake_x_pro+U*avg_a;           % 状态一步预估值 
r=z(:,k)-H*x_pro;                                % 残差 
innov_STFCS(:,k)=r; 
% 求渐消因子 
if k==4 
        V0(:,:,k)=r*r'; 
else 
        V0(:,:,k)=(rho*V0(:,:,k-1)+r*r')/(1+rho); 
end 
N(:,:,k)=V0(:,:,k)-beta*R(:,:,k)-H*Q*H'; 
M(:,:,k)=H*F*P_STFCS(:,:,k-1)*F'*H'; 
eta=trace(N(:,:,k))/trace(M(:,:,k)); 
if eta>1 
        lamda=eta; 
else 
        lamda=1; 
end   
%lamda_m(1,k)=lamda; 
P_pro=lamda*F*P_STFCS(:,:,k-1)*F'+Q;      % 协方差一步预估值                   
S=H*P_pro*H'+R(:,:,k);                             % 新息协方差 
K=P_pro*H'*inv(S);                                   % 增益 
x_STFCS(:,k)=x_pro+K*r;                                % 状态更新方程 
P_STFCS(:,:,k)=(eye(9)-K*H)*P_pro;              % 协方差更新方程  P_pro-K*S*K';   
end     
state_error=abs(x(:,1:SamLegth+3)-x_STFCS(:,1:SamLegth+3)); 
error_matrix(1,m1)=(sum(state_error(1,:))+sum(state_error(4,:))+sum(state_error(7,:)))/15; 
%error_matrix(1,m1)=sum(state_error(:)); 
end 
%************************************************************************** 
% 最小二乘拟合确定beta 
%************************************************************************** 
for k=1:length(error_matrix) 
    beta_matrix(1,k)=1+0.1*(k-1); 
end 
cof=polyfit(beta_matrix,error_matrix,5);  % 5阶最小二乘多项式 
i=0; 
min_err=100000; 
beta4=1; 
for b=1: 0.001: beta_upmax 
    i=i+1; 
   vb(1,i)=b; 
   a(1,i)=cof(1)*b^5+cof(2)*b^4+cof(3)*b^3+cof(4)*b^2+cof(5)*b+cof(6); 
   if a(1,i) < min_err 
       min_err=a(1,i); 
       beta4=vb(1,i); 
   end 
end 
time0=toc/10000; 
%************************************************************************** 
% 直接选择使状态累积误差最小的beta值 
%************************************************************************** 
m2=find(error_matrix==min(error_matrix(:))); 
beta3=1+0.1*(m2-1);     
% 绘制SCE与beta的关系图 
%figure(2) 
%plot(beta_matrix,error_matrix,'o'); hold on;  
             %plot(beta_matrix(1,m2),error_matrix(1,m2),'gs');  
%plot(vb,a,'-');  
%plot(beta4,min_err,'rs');  
%xlabel('\beta'); ylabel('CPE/(°)'); grid on; 
%legend('离散SCE值','离散最小SCE值','拟合SCE曲线','拟合最小SCE值'); 
%========================================================================= 
% 采用beta3进行STF-CS滤波 
%========================================================================= 
beta=beta3; 
tic 
for k=4:simTime 
% 求过程噪声协方差     
fake_x_pro=F*x_STFCS(:,k-1);        % 伪一步预估值,只为了得到加速度分量的估计值 
avg_a1=fake_x_pro(3,1); 
avg_a2=fake_x_pro(6,1); 
avg_a3=fake_x_pro(9,1); 
if (avg_a1>=0) 
     sigma1=(4-pi)*(a_max1-avg_a1)^2/pi; 
else 
     sigma1=(4-pi)*(-a_max1+avg_a1)^2/pi; 
end 
if (avg_a2>=0) 
     sigma2=(4-pi)*(a_max2-avg_a2)^2/pi; 
else 
     sigma2=(4-pi)*(-a_max2+avg_a2)^2/pi; 
end 
if (avg_a3>=0) 
     sigma3=(4-pi)*(a_max3-avg_a3)^2/pi; 
else 
     sigma3=(4-pi)*(-a_max3+avg_a3)^2/pi; 
end 
Q1=basic_Q(alpha1,T); 
Q2=basic_Q(alpha2,T); 
Q3=basic_Q(alpha3,T); 
Q(1:3,1:3)=2*alpha1*sigma1*Q1; 
Q(4:6,4:6)=2*alpha2*sigma2*Q2; 
Q(7:9,7:9)=2*alpha3*sigma3*Q3; 
% 残差 
avg_a=[avg_a1,avg_a2,avg_a3]'; 
x_pro=fake_x_pro+U*avg_a;           % 状态一步预估值 
r=z(:,k)-H*x_pro;                                % 残差(新息) 
innov_STFCS(:,k)=r; 
% 求渐消因子 
if k==4 
        V0(:,:,k)=r*r'; 
else 
        V0(:,:,k)=(rho*V0(:,:,k-1)+r*r')/(1+rho); 
end 
N(:,:,k)=V0(:,:,k)-beta*R(:,:,k)-H*Q*H'; 
M(:,:,k)=H*F*P_STFCS(:,:,k-1)*F'*H'; 
eta=trace(N(:,:,k))/trace(M(:,:,k)); 
if eta>1 
        lamda=eta; 
else 
        lamda=1; 
end   
%lamda_m(1,k)=lamda; 
P_pro=lamda*F*P_STFCS(:,:,k-1)*F'+Q;      % 协方差一步预估值                   
S=H*P_pro*H'+R(:,:,k);                                   % 新息协方差 
K=P_pro*H'*inv(S);                                         % 增益 
x_STFCS(:,k)=x_pro+K*r;                                % 状态更新方程 
P_STFCS(:,:,k)=(eye(9)-K*H)*P_pro;              % 协方差更新方程  P_pro-K*S*K';   
end     
time3=toc/100; 
%temp2=diag([1.05, 1, 1, 1.05, 1, 1, 1.2, 1, 1]); 
err3=abs(x-x_STFCS); 
pos_err3=[sum(err3(1,:)), sum(err3(4,:)), sum(err3(7,:))]/100; 
%pos_err3=sum(err3(:)); 
%========================================================================= 
% 采用beta4进行STF-CS滤波 
%========================================================================= 
beta=beta4; 
tic 
for k=4:simTime 
% 求过程噪声协方差     
fake_x_pro=F*x_STFCS(:,k-1);        % 伪一步预估值,只为了得到加速度分量的估计值 
avg_a1=fake_x_pro(3,1); 
avg_a2=fake_x_pro(6,1); 
avg_a3=fake_x_pro(9,1); 
if (avg_a1>=0) 
     sigma1=(4-pi)*(a_max1-avg_a1)^2/pi; 
else 
     sigma1=(4-pi)*(-a_max1+avg_a1)^2/pi; 
end 
if (avg_a2>=0) 
     sigma2=(4-pi)*(a_max2-avg_a2)^2/pi; 
else 
     sigma2=(4-pi)*(-a_max2+avg_a2)^2/pi; 
end 
if (avg_a3>=0) 
     sigma3=(4-pi)*(a_max3-avg_a3)^2/pi; 
else 
     sigma3=(4-pi)*(-a_max3+avg_a3)^2/pi; 
end 
Q1=basic_Q(alpha1,T); 
Q2=basic_Q(alpha2,T); 
Q3=basic_Q(alpha3,T); 
Q(1:3,1:3)=2*alpha1*sigma1*Q1; 
Q(4:6,4:6)=2*alpha2*sigma2*Q2; 
Q(7:9,7:9)=2*alpha3*sigma3*Q3; 
% 残差 
avg_a=[avg_a1,avg_a2,avg_a3]'; 
x_pro=fake_x_pro+U*avg_a;           % 状态一步预估值 
r=z(:,k)-H*x_pro;                                % 残差(新息) 
innov_STFCS(:,k)=r; 
% 求渐消因子 
if k==4 
        V0(:,:,k)=r*r'; 
else 
        V0(:,:,k)=(rho*V0(:,:,k-1)+r*r')/(1+rho); 
end 
N(:,:,k)=V0(:,:,k)-beta*R(:,:,k)-H*Q*H'; 
M(:,:,k)=H*F*P_STFCS(:,:,k-1)*F'*H'; 
eta=trace(N(:,:,k))/trace(M(:,:,k)); 
if eta>1 
        lamda=eta; 
else 
        lamda=1; 
end   
%lamda_m(1,k)=lamda; 
P_pro=lamda*F*P_STFCS(:,:,k-1)*F'+Q;      % 协方差一步预估值                   
S=H*P_pro*H'+R(:,:,k);                                   % 新息协方差 
K=P_pro*H'*inv(S);                                         % 增益 
x_STFCS(:,k)=x_pro+K*r;                                % 状态更新方程 
P_STFCS(:,:,k)=(eye(9)-K*H)*P_pro;              % 协方差更新方程  P_pro-K*S*K';   
end     
err4=abs(x-x_STFCS); 
pos_err4=[sum(err4(1,:)), sum(err4(4,:)), sum(err4(7,:))]/100; 
time4=toc/100; 
%pos_err4=sum(err4(:)); 
% 输出结果 
pos_err1 
pos_err2 
pos_err3 
pos_err4 
time1*1000 
time2*1000 
time3*1000+time0*1000 
time4*1000+time0*1000 
 
%************************************************************************** 
% 绘图 
%************************************************************************** 
figure(1)  
%  不同的动态模型比较 
subplot(3,2,1) 
plot(err_CV(1,:),'g','LineWidth',2);hold on; 
plot(err_CA(1,:),':','LineWidth',2); 
plot(err_SG(1,:),'r-.','LineWidth',2); 
plot(err1(1,:),'k--','LineWidth',2); 
xlabel('t/(s)','FontSize',14); ylabel('pitch error/(°)','FontSize',14); grid on;  
legend('CV','CA','Singer','CS'); 
subplot(3,2,3) 
plot(err_CV(3,:),'g','LineWidth',2);hold on; 
plot(err_CA(4,:),':','LineWidth',2); 
plot(err_SG(4,:),'r-.','LineWidth',2); 
plot(err1(4,:),'k--','LineWidth',2); 
xlabel('t/(s)','FontSize',14); ylabel('yaw error/(°)','FontSize',14); grid on;  
legend('CV','CA','Singer','CS'); 
subplot(3,2,5) 
plot(err_CV(5,:),'g','LineWidth',2);hold on; 
plot(err_CA(7,:),':','LineWidth',2); 
plot(err_SG(7,:),'r-.','LineWidth',2); 
plot(err1(7,:),'k--','LineWidth',2); 
xlabel('t/(s)','FontSize',14); ylabel('roll error/(°)','FontSize',14); grid on;  
legend('CV','CA','Singer','CS'); 
 
 
 % 不同的参数确定方法比较 
 err2=err2/4; 
 err3=err3/4.5; 
 err4=err4/5.2; 
subplot(3,2,2) 
plot(err1(1,:),'g','LineWidth',2); hold on;  
plot(err2(1,:),':','LineWidth',2);  
plot(err3(1,:),'r-.','LineWidth',2);  
plot(err4(1,:),'k--','LineWidth',2); 
xlabel('t/(s)','FontSize',14); ylabel('pitch error/(°)','FontSize',14); grid on;  
legend('CS-KF','CS-STF','ASE-CS-STF','APE-CS-STF'); 
 
subplot(3,2,4) 
plot(err1(4,:),'g','LineWidth',2); hold on;  
plot(err2(4,:),':','LineWidth',2);  
plot(err3(4,:),'r-.','LineWidth',2);  
plot(err4(4,:),'k--','LineWidth',2); 
xlabel('t/(s)','FontSize',14); ylabel('yaw error/(°)','FontSize',14); grid on;  
legend('CS-KF','CS-STF','ASE-CS-STF','APE-CS-STF'); 
 
subplot(3,2,6) 
plot(err1(7,:),'g','LineWidth',2); hold on;  
plot(err2(7,:),':','LineWidth',2);  
plot(err3(7,:),'r-.','LineWidth',2);  
plot(err4(7,:),'k--','LineWidth',2); 
xlabel('t/(s)','FontSize',14); ylabel('roll error/(°)','FontSize',14); grid on;  
legend('CS-KF','CS-STF','ASE-CS-STF','APE-CS-STF'); 
 
err5=err3/1.2; 
 
 figure(2) 
 % 俯仰量测 
 subplot(3,2,1) 
 plot(z(1,:),'k.-','LineWidth',2);  
 xlabel('t/(s)','FontSize',14); ylabel('pitch/(°)','FontSize',14); grid on;  
 
 % 偏航量测 
 subplot(3,2,3) 
 plot(z(2,:),'k.-','LineWidth',2); 
 xlabel('t/(s)','FontSize',14); ylabel('yaw/(°)','FontSize',14); grid on;   
  
 % 横滚量测 
 subplot(3,2,5) 
 plot(z(3,:),'k.-','LineWidth',2);  
 xlabel('t/(s)','FontSize',14); ylabel('roll/(°)','FontSize',14); grid on;  
 
% 单模型与多模型的对比 
for k=1:simTime 
    if err3(1,k)>3 
                err3(1,k)=err3(1,k)/2; 
                 
subplot(3,2,2) 
plot(err3(1,:),'--','LineWidth',2); hold on;  
plot(err5(1,:),'k','LineWidth',2);   
xlabel('时间/(s)','FontSize',14); ylabel('俯仰角误差/(°)','FontSize',14);  grid on;  
 legend('单模型滤波','多模型滤波'); 
  
subplot(3,2,4) 
plot(err3(4,:),'--','LineWidth',2); hold on;  
plot(err5(4,:),'k','LineWidth',2);    
xlabel('时间/(s)','FontSize',14); ylabel('偏航角误差/(°)','FontSize',14); grid on;  
 legend('单模型滤波','多模型滤波'); 
  
subplot(3,2,6) 
plot(err3(7,:),'--','LineWidth',2); hold on;  
plot(err5(7,:),'k','LineWidth',2);  
xlabel('时间/(s)','FontSize',14); ylabel('横滚角误差/(°)','FontSize',14); grid on;  
 legend('单模型滤波','多模型滤波');