known_input.asv

kalman滤波

clear
% The number of DDOF
n=3;
%dt=input('Time step  '); % When f(t) is asuumed to be constant in [k*dt (k+1)*dt] dt must be small   
dt=0.02;

% Measured acceleration responses
load response.mat
[ll,nn]=size(acc);
sensor_num=nn-1;

% Measured external excitation
% load force.mat
% f=force(:,2); 

% load elcentro.txt
% f=elcentro(:,2); 

% dd=eye(n); % Location of Sensors
dd=zeros(2,3); dd(1,1)=1.0; dd(2,2)=1.0; % Meaurements of acceleration responses at the 1st and 3th floor

% mass matrix:  m1=6kg, m2=5kg, m3=4kg 
m(1)=6; m(2)=5; m(3)=4;
mass=diag(m);

% Initial values
xk(1:2*n,1)=zeros(2*n,1);  % Initial values of displacements and velocities
xk(2*n+1:3*n,1)=2*10^2*ones(n,1);  % Initial values of stiffness 
xk(3*n+1:4*n,1)=ones(n,1);  % Initial values of damping 

pk=zeros(4*n);
pk(1:2*n,1:2*n)=eye(2*n);  % Initial values for error covariance of matrix 
pk(2*n+1:3*n,2*n+1:3*n)=10^6*eye(n);  % Initial values of stiffness error covariance matrix
pk(3*n+1:4*n,3*n+1:4*n)=10^2*eye(n);  % Initial values of damping error covariance matrix 

rk=.1*eye(sensor_num); % measurement noise 

% Recursive Solution

for k=1:ll-1;
 
% Extended Kalman Filter
  [xk_1,pk_1]=klm(n,xk,pk,f,acc,rk,k,dt,mass,dd);
  k
  [xk(2*n+1:4*n) xk_1(2*n+1:4*n)]
  pk=pk_1;
  xk=xk_1;
  
end