系统参数变化_用集合卡尔曼滤波器估计其变化_matlab

clear all clc K=20; T=1; ts=0.1; %采样时间 N=10; %集合10个样本 x1=zeros(1,100); %采样100个点，存放真值 x2=zeros(1,100); y=zeros(1,100); X1=zeros(1,10); %存放10个样本 X2=zeros(1,10); %Y=zeros(1,10); X=zeros(2,10); Y=zeros(2,100); %存放集合卡尔曼估计出来的值 %r1=[ -1.3499 3.0349 0.7254 -0.0631 0.7147 -0.2050 -0.1241 1.4897 1.4090 1.4172]; %r2=[ -1.3499 3.0349 0.7254 -0.0631 0.7147 -0.2050 -0.1241 1.4897 1.4090 1.4172]; r1=1*randn(1,10); %随机产生10个样本 r2=1*randn(1,10); v=1*randn(1,100); %测量噪声 w=1*randn(1,100); %过程噪声 %w=[0.0888 -0.1147 -0.1069 -0.0809 -0.2944 0.1438 0.0325 -0.0755 0.1370 -0.1712 -0.0102 -0.0241 0.0319 0.0313 -0.0865 -0.0030 -0.0165 0.0628 0.1093 0.1109 -0.0864 0.0077 -0.1214 -0.1114 -0.0007 0.1533 -0.0770 0.0371 -0.0226 0.1117 -0.1089 0.0033 0.0553 0.1101 0.1544 0.0086 -0.1492 -0.0742 -0.1062 0.2350 -0.0616 0.0748 -0.0192 0.0889 -0.0765 -0.1402 -0.1422 0.0488 -0.0177 -0.0196 0.1419 0.0292 0.0198 0.1588 -0.0804 0.0697 0.0835 -0.0244 0.0216 -0.1166 -0.1148 0.0105 0.0722 0.2585 -0.0667 0.0187 -0.0082 -0.1933 -0.0439 -0.1795 0.0840 -0.0888 0.0100 -0.0545 0.0304 -0.0600 0.0490 0.0739 0.1712 -0.0194 -0.2138 -0.0840 0.1355 -0.1072 0.0961 0.0124 0.1437 -0.1961 -0.0198 -0.1208 0.2908 0.0825 0.1379 -0.1058 -0.0469 -0.0272 0.1098 -0.0278 0.0702 -0.2052]; %v=[0.0888 -0.1147 -0.1069 -0.0809 -0.2944 0.1438 0.0325 -0.0755 0.1370 -0.1712 -0.0102 -0.0241 0.0319 0.0313 -0.0865 -0.0030 -0.0165 0.0628 0.1093 0.1109 -0.0864 0.0077 -0.1214 -0.1114 -0.0007 0.1533 -0.0770 0.0371 -0.0226 0.1117 -0.1089 0.0033 0.0553 0.1101 0.1544 0.0086 -0.1492 -0.0742 -0.1062 0.2350 -0.0616 0.0748 -0.0192 0.0889 -0.0765 -0.1402 -0.1422 0.0488 -0.0177 -0.0196 0.1419 0.0292 0.0198 0.1588 -0.0804 0.0697 0.0835 -0.0244 0.0216 -0.1166 -0.1148 0.0105 0.0722 0.2585 -0.0667 0.0187 -0.0082 -0.1933 -0.0439 -0.1795 0.0840 -0.0888 0.0100 -0.0545 0.0304 -0.0600 0.0490 0.0739 0.1712 -0.0194 -0.2138 -0.0840 0.1355 -0.1072 0.0961 0.0124 0.1437 -0.1961 -0.0198 -0.1208 0.2908 0.0825 0.1379 -0.1058 -0.0469 -0.0272 0.1098 -0.0278 0.0702 -0.2052]; for k=1:100 u(k)=1; end x10=0; x20=K; R=1; H=[1,0]; X0=[x10+r1;x20+r2]; %初始集合 X1=X0(1,:); %赋初值 X2=X0(2,:); x1(1,1)=x10; x2(1,1)=x20; y(1,1)=0; for t=1:100 he1=0; he2=0; he3=0; he4=0; % 一阶系统（把K扩展为状态） x1(1,t+1)=x1(1,t)-ts/T*x1(1,t)+x2(1,t)*ts/T*u(t)+w(t); x2(1,t+1)=x2(1,t)+w(t); y(1,t+1)=x1(1,t+1)+v(t); for k=1:N X1(1,k)=X1(1,k)-ts/T*X1(1,k)+X2(1,k)*ts/T*u(k); %集合预报 X2(1,k)=X2(1,k); y1(1,k)=y(1,t)+v(t); %有噪声的测量 %Y(1,k+10)=X1(1,k+1); end for i=1:10 he1=he1+X1(1,i); he2=he2+X2(1,i); end xp1=1*(he1)/N; xp2=1*(he2)/N; xp=[xp1;xp2]; %集合平均 X=[X1;X2]; for j=1:10 XP(:,j)=(X(:,j)-xp); %偏差矩阵 end B=XP*XP'*inv(N-1); %背景误差协方差 Kg=B*H'*inv(H*B*H'+R); %增益矩阵 X=X+Kg*(y1-H*X); %集合修正 for a=1:10 he3=he3+X(1,a); he4=he4+X(2,a); end Y(1,t)=he3/N; %EnKF估计输出 Y(2,t)=he4/N; error(1,t)=(x2(1,t)-Y(2,t))/x2(1,t); end figure(1) t = 1 : 100; subplot(2,1,1) plot(t,x2(1,t),'b.',t,Y(2,t),'r.');hold on; % set(gca,'FontSize',10); set(gcf,'Color','White'); xlabel('采样'); ylabel('x2 state'); legend('真实值', 'EnKF估计值'); %figure(2) %t = 1 : 100; subplot(2,1,2) plot(t,error(1,t),'k-');hold on; % set(gca,'FontSize',10); set(gcf,'Color','White'); xlabel('采样'); ylabel('相对误差'); %legend('真实值', 'EnKF估计值'); % figure(3) % t = 1 : 100; % %subplot(2,1,1) % plot(t,x1(1,t),'b.',t,Y(1,t),'k-');hold on; % % set(gca,'FontSize',10); set(gcf,'Color','White'); % xlabel('采样'); ylabel('x1 state'); % legend('真实值', 'EnKF估计值');