kcm.m

kalman滤波

function [stiff,damp,x3p,x4p]=kcm(n,xk)

% Stiffness Matrix
k(1:n)=xk(2*n+1:3*n);  % Stiffness parameters

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:n)=xk(3*n+1:4*n); % Damping parameters

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);

% xpie3 matrix
x=xk(1:n);  % Displacement 

x3p=zeros(n);
x3p(1,1)=x(1); x3p(1,2)=x(1)-x(2);
for i=2:n-1;
   x3p(i,i)=x(i)-x(i-1);
   x3p(i,i+1)=x(i)-x(i+1);
end      
x3p(n,n)=x(n)-x(n-1); 


% xpie4 matrix
xp=xk(n+1:2*n);  %Velocity 

x4p=zeros(n);
x4p(1,1)=xp(1); x4p(1,2)=xp(1)-xp(2);
for i=2:n-1;
   x4p(i,i)=xp(i)-xp(i-1);
   x4p(i,i+1)=xp(i)-xp(i+1);
end      
x4p(n,n)=xp(n)-xp(n-1);