klm.m

kalman滤波

function [xk_1,pk_1]=klm(n,xk,pk,f,acc,rk,k,dt,mass,dd,te)
%kalman filter

% The predicated error covariance matrix
A=zeros(4*n);  % State transition matrix
A(1:n,n+1:2*n)=eye(n);
mm=inv(mass);
[stiff,damp,x3p,x4p]=kcm(n,xk);
A(n+1:2*n,1:4*n)=-mm*[stiff,damp,x3p, x4p];

fi=eye(4*n)+dt*A;

pbk_1=fi*pk*fi.';

% The predicted extended state vector by numerical integration

OPTIONS = [];
pre=ode45(@predict,[dt*k dt*(k+1)],xk,OPTIONS,n,mass,te);
%pre=ode45(@predict2,te,xk,OPTIONS,n,mass,k); % Assume f(t) is constant during [k*dt (k+1)*dt], so dt must be small 
xbk_1=pre.y(:,end);

% Observation equations at time (k+1)*dt
% Measured acceleration responses
 yk_1=dd*acc(k+1,:)';

% Function h at time (k+1)*dt
[stiff,damp,x3p,x4p]=kcm(n,xbk_1);
fk_1=zeros(n,1);
fk_1(n)=f(k+1);
h=dd*inv(mass)*(fk_1-stiff*xbk_1(1:n)-damp*xbk_1(n+1:2*n));
hk_1=dd*inv(mass)*[-stiff,-damp,-x3p, -x4p]; 

% Kalman gain matrix
k_1=pbk_1*hk_1'*inv(hk_1*pbk_1*hk_1'+rk);

% The filted extended state vector 
xk_1=xbk_1+k_1*(yk_1-h);

% The filted error covariance matrix 
pk_1=(eye(4*n)-k_1*hk_1)*pbk_1;