www.pudn.com > strong-tracking-filter.rar > stf_cs_para.m, change:2010-10-09,size:10287b


% 
%  STF-CS, 研究APE与rho和beta的关系 
%  
clear all; 
clc; 
echo off; 
%************************************************************************ 
% 可调节参数汇总 
%************************************************************************ 
mea_noise_m=10;                % 量测噪声均值 
alpha=0.05;                               % 机动频率1,1/20,1/60 
a_max=2;                                % 加速度极限值 
rho=0.95;                                 % 残差方差阵的遗忘因子初始值 (0.95<rho<0.995) 
beta_upmax=8;                       % 衰减因子选定极限 
%========================================================================= 
% 载入或生成量测数据 
%========================================================================= 
% 载入量测数据 
%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)=55; 
x(7,20)=70; 
x(6,20)=1; 
%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)=-70; 
x(5,60)=0; 
x(6,60)=-1; 
%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预置协方差空间 
innov_CS=zeros(3,simTime);            % CS新息序列 
innov_STFCS=zeros(3,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); 
% rho=0.95;                                     % 残差方差阵的遗忘因子(0<rho<=1) 
% beta=1.5;                                     % 量测噪声的衰减因子(beta>=1) 
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; 
%========================================================================= 
% 分析APE与rho及beta的关系 
%========================================================================= 
SamLegth=97;    % SamLegth < simTime-3 
m1=0; 
for rho=0.95:0.002:0.995   % 46  0.001 
      m1=m1+1; 
      m2=0; 
for beta=1: 1.5: beta_upmax   % 71   0.1 
        m2=m2+1; 
        lamda=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(m1,m2)=(sum(state_error(1,:))+sum(state_error(4,:))+sum(state_error(7,:)))/10; 
end 
end 
% 选择使状态累积误差最小的rho和beta值 
min_err=min(error_matrix(:)); 
[k1,k2]=find(error_matrix==min_err); 
rho1=0.95+0.001*(k1-1) 
beta1=1+0.1*(k2-1)    
%========================================================================= 
% % 分析APE与beta的关系 
%========================================================================= 
rho=0.95; 
m3=0; 
for beta=1: 0.1: beta_upmax   % 41 
        m3=m3+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_error1=abs(x(:,1:SamLegth+3)-x_STFCS(:,1:SamLegth+3)); 
error_matrix1(1,m3)=(sum(state_error1(1,:))+sum(state_error1(4,:))+sum(state_error1(7,:)))/10; 
end 
%************************************************************************** 
% 最小二乘拟合确定beta 
%************************************************************************** 
for k=1:length(error_matrix1) 
    beta_matrix(1,k)=1+0.1*(k-1); 
end 
cof=polyfit(beta_matrix,error_matrix1,5);  % 5阶最小二乘多项式 
i=0; 
min_err=100000; 
beta2=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); 
       beta2=vb(1,i); 
   end 
end 
 beta2 
% 绘制SCE与rho和beta的关系图 
figure(1) 
plot(error_matrix(:),'k.','LineWidth',2); 
xlabel('\rho, \beta','FontSize',14); ylabel('APE','FontSize',14); grid on; 
 
 
 
figure(2) 
plot(beta_matrix,error_matrix1,'ko','LineWidth',1);  hold on 
plot(vb,a,'k','LineWidth',2); 
plot(beta2,min_err,'ks','LineWidth',2);  
xlabel('\beta','FontSize',14); ylabel('APE','FontSize',14); grid on; 
legend('样本点','拟合曲线','最小值');