linear3dof.asv

kalman滤波

function yp=linear3dof(tb,yb,nn);
% The 3-DOF Linear System
n=3;

% mass matrix
% m1=2kg, m2=1.5kg, m3=1.0kg 
m(1)=2; m(2)=1.5; m(3)=1.0;
mass=diag(m);

% Stiffness Matrix
% k1=1800N/m, k2=1200N/m, k3=600N/m, 
k(1)=1800;k(2)=1200;k(3)=600;

stiff=zeros(n);
stiff(1,1)=k(1)+k(2); stiff(1,2)=-k(2);

for i=2:n-1;
   stiff(i,i-1)=-k(i);
   stiff(i,i)=k(i)+k(i+1);
   stiff(i,i+1)=-k(i+1);
end      
stiff(n,n-1)=-k(n); stiff(n,n)=k(n); 

% Damping Matrix
% c(1)=1.0N/m, c(2)=0.8N/m, c(3)=0.7N/m, 
c(1)=0.9;c(2)=0.7;c(3)=0.6;

damp=zeros(n);
damp(1,1)=c(1)+c(2); damp(1,2)=-c(2);

for i=2:n-1;
   damp(i,i-1)=-c(i);
   damp(i,i)=c(i)+c(i+1);
   damp(i,i+1)=-c(i+1);
end      
damp(n,n-1)=-c(n); damp(n,n)=c(n);
   
% State transition matrix
A=zeros(n);
A(1:n,n+1:2*n)=eye(n);
A(n+1:2*n,1:n)=-inv(mass)*stiff;
A(n+1:2*n,n+1:2*n)=-inv(mass)*damp;
   
% Excitation influence matrix with excitation on the 3rd floor
B=zeros(2*n,1);
B(n+1:2n)=inv(mass)*1;
  
% White noise excitation
load force.mat 
u=interp1(force(:,1),force(:,2),tb,'*spline');
yp = A * yb +inv(mass)*B * u;