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;
%=========================================================================
% 载入或生成量测数据
%=========================================================================
% 载入量测数据
%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('单模型滤波','多模型滤波');