predict.m

kalman滤波

function yp=predict(tb,yb,n,mass,te);
% The 3-DOF Linear System

% functions of generalized state equations
% excitation force acting on the top floor at time k*dt
fk=zeros(n,1);

% load force.mat
% fk(n,1)=interp1(force(:,1),force(:,2),tb);

% load elcentro.txt
% fk(n,1)=interp1(elcentro(:,1),elcentro(:,2),tb);

% Sweep sine excitation at the 3rd floor
amp=20.0;
f0=0.2;
fe=12.0;
fk(n)=amp*sin((f0+(fe-f0)/2/te*tb)*tb);

% Function g
g(1:n,1)=yb(n+1:2*n);
[stiff,damp,x3p,x4p]=kcm(n,yb);
g(n+1:2*n)=inv(mass)*(fk-stiff*yb(1:n)-damp*yb(n+1:2*n));
g(2*n+1:4*n)=zeros(2*n,1);

yp =g;