clear;

X(1,1)=0.950;

X(2,1)=0.004;

X(3,1)=0.004;%初值

R = 0.0001;

tf = 4000; %仿真长度

N = 1000; %粒子数目

P = 0.0001;

XhatPart1 = X(1,1);    %xhatPart=x=0.1

XhatPart2 = X(2,1);

XhatPart3 = X(3,1);

% Initialize the particle filter. 初始化粒子滤波，xpart值用来在不同时刻生成粒子

for i = 1 : N

Xpart(1,i) = X(1,1)+ wgn(1,1,10*log10(0.0001));

Xpart(2,i) = X(2,1)+ wgn(1,1,10*log10(0.0001));

Xpart(3,i) = X(3,1)+ wgn(1,1,10*log10(0.0001));

end      %初始化xpart(i)为生成的随机粒子(有时考虑到精度会手动初始化)

XArr1 = X(1,1);    %存放数组

XArr2 = X(2,1);

XArr3 = X(3,1);

XhatPartArr1 = XhatPart1;   %xhatPartArr = [xhatPart]=0.1；存放数组

XhatPartArr2 = XhatPart2;

XhatPartArr3 = XhatPart3;

close all;%close all 是关闭所有窗口(程序运行产生的，不包括命令窗，editor窗和帮助窗)

%clear all是清除所有工作空间中的变量

for k = 1 : tf %tf为时间长度，k可以理解为时间轴上的k时刻；Tf=50

% System simulation系统仿真

% x数据为时刻k的真实状态值

A=[0 0 0;0 -0.03 0;0 0 -0.003];

B=[-0.0003;-0.0014;-0.0003];

g(k)=-1.031*exp(-35*X(1,k))+3.685+0.2156*X(1,k)-0.1178*X(1,k)^2+0.3201*X(1,k)^3;

[G,H]=c2d(A,B,1);%离散化

X(:,k+1)=G*X(:,k)+0.08*H;%+wgn(3,1,10*log10(0.0001));这里不加系统噪声

Y(k)=g(k)+X(2,k)+X(3,k)+0.08*0.07;%+wgn(1,1,10*log10(0.0001));%这里也不加观测噪声

for i = 1 : N

Xpartminus(:,i)=G*Xpart(:,i)+0.08*H+wgn(3,1,10*log10(0.0001));

Ypart=-1.031*exp(-35*Xpartminus(1,i))+3.685+0.2156*Xpartminus(1,i)-0.1178*Xpartminus(1,i)^2+0.3201*Xpartminus(1,i)^3+Xpartminus(2,i)+Xpartminus(3,i)+0.08*0.07;

vhat = Y(k)-Ypart;%

q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R); %根据差值给出1粒子对应的权重

end

% Normalize the likelihood of each a priori estimate.       每一个先验估计正常化的可能性

qsum = sum(q);

for i = 1 : N

q(i) = q(i) / qsum;

end

for i = 1 : N

u = rand; % uniform random number between 0 and 1,0和1之间的均匀随机数;rand函数产生由在(0, 1)之间均匀分布的随机数组成的数组。

qtempsum = 0;

for j = 1 : N

qtempsum = qtempsum + q(j);

if qtempsum >= u

%重采样对低权重进行剔除，同时保留高权重，防止退化的办法

%part(i) = xpartminus(j);

Xpart(1,i)=Xpartminus(1,j);

break;

end

end

end

% The particle filter estimate is the mean of the particles.      粒子滤波的估计是颗粒的平均值

test=mean(Xpart(1,:));

Xpart(1,k+1) = test; %经过粒子滤波处理后的均值;mean(A)

%当A为向量时，那么返回值为该向量所有元素的均值

%当A为矩阵时，那么返回值为该矩阵各列向量的均值

%mean(A,2)

%返回值为该矩阵的各行向量的均值

% Plot the estimated pdf's at a specific time.      绘制在特定的时间估计的概率密度函数

if k == 20

% Particle filter pdf

pdf = zeros(81,1);% zeros，返回一个m x n x px...的矩阵,所有元素初始化为0。

for m = -40 : 40   % 为什么这么取呢

for i = 1 : N

if (m <= Xpart(1,i)) && (Xpart(1,i) < m+1)

%pdf为概率密度函数，这里是xpart(i)值落在[m, m+1)上的次数

pdf(m+41) = pdf(m+41) + 1;

end

end

end

end

% Save data in arrays for later plotting

XArr1 =[XArr1 X(1,k+1)];%

XhatPart1 =[XhatPart1 Xpart(1,k+1)];%

end

t = 0 : tf;

figure;

plot( t,XArr1, 'g',t, XhatPart1,'r'); %此图2对应xArr为真值，xhatPartArr为粒子滤波值%t, XArr1, 'b.',

xlabel('time step'); ylabel('state');

legend('True state', 'Particle filter estimate');

此算法是根据网上比较流行的粒子滤波算法改动的，在这里有三个状态变量，但我最终需要估计的是Ｘ１；事实上，系统矩阵均是动态的，但这里人为给固定了，成了非线性定常问题；　　仿真出现的问题是：如果加系统误差以及观测误差的话，测量值非常紊乱，我做的是电池的SOC估计，而且是放电，因此ＳＯＣ(在这里也即Ｘ１)应该是从１降到０左右才对，测量值不对，估计因此也不对；但是如果我把这两个噪声去掉的话，完全是真值，那么估计值慢慢收敛，但在收敛之后又发散了，不知为何，现在怀疑程序有问题，求高手指点！十分感谢！(代码直接复制仿真即可，由于仿真比较慢，可以把仿真时间改动一下，这里是４０００，步长为１)