%%基本粒子滤波器 参数时变 SOC 估计 DST 工况 rand('seed',1); randn('seed',1);%给定随机数的种子点 %%真实值给定 load EKF2.mat; I=ekf2(2,:); Ut=ekf2(3,:); R0=ekf2(4,:); R1=ekf2(5,:); R2=ekf2(6,:); C1=ekf2(7,:); C2=ekf2(8,:); SOC=ekf2(9,:); Cn=16.8; T=length(Ut);%%循环次数 QQ=10^(-4.5)*eye(3); Q=10^(-10)*diag([10^(-3),10^(-7),10^(-7)]); R=0.003; X0=[0.8;0;0]; %%粒子滤波器状态初始化 N=1000; Xpf=zeros(3,T); Xpf(:,1)=X0; Zpf=zeros(1,T);%%滤波器滤波后的观测 Zpf 与 Xpf 对应 %%粒子集初始化 Xm=zeros(3,N,T); Zm=zeros(1,N,T);%%滤波器预测观测 Zm 与 Xm 对应 for i=1:N %%粒子数 Xm(:,i,1)=X0+sqrtm(Q)*randn(3,1); Zm(1,i,1)=Ut(1); end %观测量 Z=Ut; W=zeros(T,N);%%权值初始化 %%粒子滤波算法 for k=2:T A=[1 0 0;0 exp(-1/(R1(k)*C1(k))) 0;0 0 exp(-1/R2(k)*C2(k))]; B=[-1*0.99/(3600*Cn);R1(k)*(1-exp(-1/(R1(k)*C1(k))));R2(k)*(1-exp(-1/(R2(k)*C2(k))))]; for i=1:N Xm(:,i,k)=A*Xm(:,i,k-1)+B*I(k-1)+sqrtm(QQ)*randn(3,1); end %%重要性权值计算 for i=1:N 

Zm(1,i,k)=11.08*Xm(1,i,k)^6-25.58*Xm(1,i,k)^5+17.54*Xm(1,i,k)^4-1.159*Xm(1,i,k)^3-2.386 *Xm(1,i,k)^2+1.263*Xm(1,i,k)+3.422-Xm(2,i,k)-Xm(3,i,k)-I(k)*R0(k);%%观测预测 W(k,i)=inv(sqrt(2*pi*det(R)))*exp(-(Z(k)-Zm(1,i,k))^2/2/R)+1e-99;%%重要性权值 end W(k,:)=W(k,:)./sum(W(k,:));%%归一化权值 Neff(k)=1/sum(W(k,:).^2); if Neff(k)>(2/3)*N %%重采样 outIndex=randomR(1:N,W(k,:)');%%随机重采样 Xm(:,:,k)=Xm(:,outIndex,k);%%得到新的样本集 else break end %%滤波器滤波后的状态更新为： Xpf(:,k)=[mean(Xm(1,:,k));mean(Xm(2,:,k));mean(Xm(3,:,k))]; %%观测更新为： Zpf(1,k)=11.08*Xpf(1,k)^6-25.58*Xpf(1,k)^5+17.54*Xpf(1,k)^4-1.159*Xpf(1,k)^3-2.386*Xpf( 1,k)^2+1.263*Xpf(1,k)+3.422-Xpf(2,k)-Xpf(3,k)-I(k)*R0(k); end figure(1) hold on plot(Xpf(1,:),'g','Linewidth',2) plot(SOC,'k','Linewidth',2) legend('PF 估计值','SOC 真实值') xlabel('time/s') ylabel('SOC') figure(2) hold on plot(Zpf(1,:),'Linewidth',2) plot(Ut,'Linewidth',2) legend('端电压估计值','端电压真实值') xlabel('time/s') ylabel('端电压/V') figure(3) plot(SOC-Xpf(1,:),'g','Linewidth',2) legend('PF 估计误差') xlabel('time/s') ylabel('SOC 估计误差') figure(4) plot(Ut-Zpf(1,:),'Linewidth',2) legend('端电压估计误差') xlabel('time/s') 

%%随机重采样 ylabel('端电压估计误差/V') function outIndex=randomR(inIndex,q) outIndex=zeros(size(inIndex)); [num,col]=size(q); u=rand(1,num); % u=rand(num,1); u=sort(u); % u=(1:num)/num; l=cumsum(q); i=1; for j=1:num while (i<=num) & (u(i)<=l(j)) outIndex(i)=j; i=i+1; end end end function outIndex=residualR(inIndex,q) if nargin<2 error('Not enough input arguments'); %%残差重采样 end [S,arb]=size(q); T_babies=zeros(1,S); q_res=S.*q'; T_babies=fix(q_res); T_res=S-sum(T_babies); if(T_res~=0) q_res=(q_res-T_babies)/T_res; cumDist=cumsum(q_res); u=fliplr(cumprod(rand(1,T_res).^(1./(T_res:-1:1)))); j=1; for i=1:T_res while(u(1,i)>cumDist(1,j)) j=j+1; end T_babies(1,j)=T_babies(1,j)+1; end index=1; for i=1:S if(T_babies(1,i)>0) for j=index:index+T_babies(1,i)-1 outIndex(j)=inIndex(i); 

%%多项式重采样 end end index=index+T_babies(1,i); end end end function outIndex=multinomialR(weight) Col=length(weight); N_babies=zeros(1,Col); cdf=cumsum(weight); u=rand(1,Col); uu=u.^(1./(Col:-1:1)); ArrayTemp=cumprod(uu); u=fliplr(ArrayTemp); j=1; for i=1:Col while (u(i)>cdf(j)) j=j+1; end N_babies(j)=N_babies(j)+1; end index=1; for i=1:Col if (N_babies(i)>0) for j=index:index+N_babies(i)-1 outIndex(j)=i; end end index=index+N_babies(i); end end