Example_Modle_Filter.rar_filter_hydraulic_液压_液压 matlab

clear all close all clc %%%%%%%%%%%%%%%%%%%%%%%%%都講%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%場宎曹講%%%%%%%%%%%%%%%%%%%% ts=0.001; PP=100*[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1]; for k=1:1:4000 time(k)=k*ts; u(k)=0.1*sin(2*3.1415*k*ts); XX(:,1)=[0 0 0 0]'; Z_1=XX(1,k); Z_2=XX(2,k); Z_3=XX(3,k); Z_4=XX(4,k); F=[1 ts 0 0;-ts*Z_4 1-ts*Z_3 -ts*Z_2 -ts*Z_1;0 0 1 0;0 0 0 1]; H=[1 0 0 0]; U=[0 ts*u(k) 0 0]'; Q=[0 0 0 0;0 0 0 0;0 0 0.0001 0;0 0 0 0.001]; I=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1]; R=3.6*10^(-6); V=0.0005*randn; Z(k)=Z_1+R*randn; K=PP*H'*inv(H*PP*H'+R); X(:,k)=XX(:,k)+K*[Z(k)-H*XX(:,k)]; P=[I-K*H]*PP; XX(:,k+1)=F*X(:,k)+U; PP=F*P*F'+Q*randn; yout(k)=XX(1,k); vout(k)=XX(2,k); dout(k)=XX(3,k); kout(k)=XX(4,k); data(:,k)=[yout(k),vout(k)]; end figure(1) subplot(2,2,1) plot(time,yout,'-r') subplot(2,2,2) plot(time,vout,'-b'); subplot(2,2,3) plot(time,dout,'-g'); subplot(2,2,4) plot(time,kout,'-g');