kf.m

来自「kalman filter program」· M 代码 · 共 91 行

%Kalman filter for linear ID(SDOF)
%test just for SOD system
%* ****************************************************************** *      _
%*                        FILE     : KFforId                          *     |_|  3rd
%*                                                                    *      |    
%*                                                                    *      _ 
%*                                                                    *     |_|  2nd
%*                             2006.10                                *      |
%**********************************************************************      _
%**********************************************************************     |_|  1st
%*                   MODEL AND PARAMETERS SETTING                     *      |                                                                         *
%*   DOT2{X(I)}+2*H(I)*W(I)*U(I)+Z(I)/M(I)-(1-DELTin)*M(I+1)/M(I)*    *   --------
%*      [2*H(I+1)*W(I+1)*DOT{U(I+1)}+Z(I+1)/M(I+1)]=-DOT2(Xg)         *   ////////
%*                                                                    *     Model    
%*                        U(I)=X(I)-X(I-1)                            *
%*                     DELTin=1,i=n,ELSE DELTin=0                     *
%*                                                                    *
%*        DOT{Z}=K*DOT{U}-ALPHA*ABS(DOT{U})*ABS{Z}**(N-1)             *
%*                       -BETA*DOT{U}*ABS{Z}**N                       *
%*                                                                    *
%*         M     :MASS                                                *
%*         N     :VERSATILE MODEL PARAMETER(WN)                       *
%*        ALPHA  :VERSATILE MODEL PARAMETER                           *
%*        BETA   :VERSATILE MODEL PARAMETER                           *
%*         H     :EQUALITY OF ATTENUATION PARAMETER                   *
%*                     H(I)=C(I)/2*SQRT(M(I)*K(I))                    *
%*         W     :EQUALITY OF NATURAL RADIAN FREQUENCY                *
%*                        W(I)=SQRT(K(I)/M(I))                        *
%*         G     :EARTHQUAKE INPUT  1 X NSTEP vector                  *
%**********************************************************************
%
%  kx+cx'=-mx"-mg
%  y=[x x'][k c]'=-m(x"+g)
%
%State Function: x(k)=x(k-1);
%Measurement Function: Z(k)=H*x(k)+v(k)
clear; clc;
echo off; 
%SYSTEM PARAMETERS
G=load('norm_ELCEN.TXT');
G=G*100;%unit gal,1g=980gal
NDOF=1;NSTEP=1000;DT=0.02;
M=0.12553;
K=24.5;
C=0.07;
ALPHA=0;
BETA=0;
WN=3;
% GENERATE THE DATA: 
% ================== 
[DISP,VEL,ACC,Z]=RungeKutta(M,K,C,NDOF,ALPHA,BETA,WN,G,NSTEP,DT);
% INITIALISATION AND PARAMETERS: 
% ============================== 
dim=2; % Number of hidden variables(state),x=[k, c]'
y=(-M*ACC);
f=inline('x');
H=[DISP',VEL'];

T = NSTEP; % Number of time steps. 

P0=[10 1;1 10];
Rv=1;
R=Rv*randn(1,T);
X=zeros(2,T);
X_elvat=zeros(2,T);
X_hat=zeros(2,T);
P=P0;
for t=2:T
    X_elvat(:,t)=X(:,t-1);
    P_elvat=P;
    KGain=P_elvat*(H(t,:)')*(inv(H(t,:)*P_elvat*H(t,:)'+R(t)));
    X_hat(:,t)=X_elvat(:,t)+KGain*(y(t)-H(t,:)*X_elvat(:,t));
    P=(eye(2)-KGain*H(t,:))*P_elvat;
    X(:,t)=X_hat(:,t);
end

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

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

% The Identification veracity in last 3/4 times
IV_K=sum((X_hat(1,0.75*T:T)-K).^2)
IV_C=sum((X_hat(2,0.75*T:T)-C).^2)