feigaosi223.m

非高斯滤波程序

clear; clc

G=load('norm_ELCEN.TXT');
G=G*100;%unit gal,1g=980gal
NDOF=1;NSTEP=1000;DT=0.02;
M=0.12553;
K=20;
C=0.8;
ALPHA=5;
BETA=5;
WN=2;

[DISP,VEL,ACC,Z]=RungeKutta(M,K,C,NDOF,ALPHA,BETA,WN,G,NSTEP,DT);

dim=6; % Number of hidden variables(state),x=[x',f,k,c,alpha,beta]'
y=VEL;

VELid(1)=0;
f_dot0id(1)=0;
Kid(1)=25;
Cid(1)=0.68;
ALPHAid(1)=6;
BETAid(1)=6.8;%Initail value

f=inline('[(-G-f_dot0id/M-Cid/M*VELid);(Kid*VELid-ALPHAid*abs(VELid)*(abs(f_dot0id))^(WN-1)*(f_dot0id)-BETAid*VELid*(abs(f_dot0id))^WN);0;0;0;0]*DT','f_dot0id','M','Cid','VELid','Kid','ALPHAid','BETAid','WN','G','DT');
% f=inline('[(-G-f_dot0id/M-Cid/M*VELid);(Kid*VELid-ALPHAid*abs(VELid)*(abs(f_dot0id))^(WN-1)-BETAid*VELid*(abs(f_dot0id))^WN);Kid;Cid;ALPHAid;BETAid]*DT','f_dot0id','M','Cid','VELid','Kid','ALPHAid','BETAid','WN','G','DT');
h=[1 0 0 0 0 0];
pe=inline('(4+x.^2/0.002^2).^(-(4+1)/2)','x');%likelihood ('(n+x.^2/s^2)^(-(n+1)/2)','x','n','s')
Q =0.0001*diag([0.03 0.03 12 0.25 8.5 6.5].^2);%Q = 0.0002*diag([0 0 20 0.27 5 5].^2);
P0 = diag([0 0 20 0.68 5.5 5.8].^2);
R = 0.0015;  
N = 600; % Number of particles. 
T = 1000; % Number of time steps. 

%[xhat,loglh,aic]=SIR(y,phi,f,h,pe,Q,P0,R,N,dim,T);
%function [xhat,loglh,aic]=SIR(y,phi,f,h,pe,Q,P0,R,N,dim,T)

xhat=sqrt(P0)*eye(dim,T);
x=[0.03*randn(1,N);0.03*randn(1,N);Kid(1)+12*randn(1,N);Cid(1)+0.1*randn(1,N);ALPHAid(1)+4.5*randn(1,N);BETAid(1)+4.5*randn(1,N)];                 % Step1, Initialize the particles
mid99=zeros(dim,N);

t=1;
f_dot0id=f_dot0id(1);Cid=Cid(1);VELid=VELid(1);Kid=Kid(1);ALPHAid=ALPHAid(1);BETAid=BETAid(1);G1=G(1);
phi=x;%feval(f,f_dot0id,M,Cid,VELid,Kid,ALPHAid,BETAid,WN,G1,DT);
 for t=7:T
    e=repmat(y(t),1,N)-h*phi;           % Step2, Calculate weights
    q=feval(pe,e);                     % the likelihood function
   
    q=q/sum(q);                          % normalize the important weights
    ind =resampling(q);                  % Step3, measurement update
    x=x(:,ind);                          % the new particles
    xhat(:,t)=(mean(x'))';               % Compute the estimate  
    
    %f_dot0id=x(2,:);Cid=x(4,:);VELid=x(1,:);Kid=x(3,:);
    %ALPHAid=x(5,:);BETAid=x(6,:);
    Gt=G(t);
    for ii=1:N
        mid99(:,ii)=feval(f,x(2,ii),M,x(4,ii),x(1,ii),x(3,ii),x(5,ii),x(6,ii),WN,Gt,DT);
    end
    mid00=sqrt(Q)*randn(dim,N);   % Step4, time update
    
    x=x+mid99+mid00;
    phi=x;
 end
 
time=(1:T)*DT;
subplot(2,2,1)
plot(time,K*ones(1,T),'b--',time,xhat(3,:),'r','lineWidth',2); %no damage
legend('TRUE','ESTIMATED'); 
xlabel('Time(s)','fontsize',8) % Plot estimation of K: 

subplot(2,2,2)
plot(time,C*ones(1,T),'b--',time,xhat(4,:),'r','lineWidth',2); %no damage
legend('TRUE','ESTIMATED'); 
xlabel('Time(s)','fontsize',8) % Plot estimation of C: 

subplot(2,2,3)
plot(time,ALPHA*ones(1,T),'b--',time,xhat(5,:),'r','lineWidth',2); %no damage
legend('TRUE','ESTIMATED'); 
xlabel('Time(s)','fontsize',8) % Plot estimation of ALPHA: 

subplot(2,2,4)
plot(time,BETA*ones(1,T),'b--',time,xhat(6,:),'r','lineWidth',2); %no damage
legend('TRUE','ESTIMATED'); 
xlabel('Time(s)','fontsize',8) % Plot estimation of BETA: