python 实现粒子滤波

1 说明

该算法是B站UP主忠厚老实的老王的代码，感兴趣的读者可移步到该处学习，本文只是将其代码用python实现
贝叶斯滤波与卡尔曼滤波第十一讲代码

2 代码实现

import random
import numpy as np
import matplotlib.pyplot as plt
t= list(range(100))
x=np.linspace(0,1,100)
#x= [0]*100
y= [0]*100
x[0]=0.1
y[0]=0.1**2

noise_size = 100
noise_array =  np.random.normal(0, 1, noise_size)

#生成真实数据与观测数据
for i in range(1,100):
    x[i]=np.sin(x[i-1]) + 5*x[i-1]/(x[i-1]**2+1)
    y[i]=x[i]**2+noise_array[i]

#PF start
#设粒子集合
n=100
xold= [0]*n
xnew= [0]*n
xplus = [0]*n
w = [0]*n

for i in range(n): 
  xold[i]=0.1
  w[i]=1/n

xold_noise_array =  np.random.normal(0, 0.1, noise_size)
#PF核心代码
for i in range(1,100):
   #预测步 由x0推出x1
   for j in range(100):
        xold[j]=np.sin(xold[j])+5*xold[j]/(xold[j]**2+1)+xold_noise_array[j]
   
   #预测步完毕   
   #更新步
   for j in range(n):
     
       w[j]=np.exp(-((y[i]-xold[j]**2)**2/(2*0.1)))
  
   #归一化
   w=w/np.sum(w)
   #w/sum(w)与k*w/sum(k*w)结果一模一样
    
   #(2*pi*R)^(-0.5)是常数
    
   #w(j)，如果每次都重采样，每次w(j)都会被设为1/n，也是常数
    
   #所以可以将他们去掉
    
   #重采样
   #N<1/sum(wi^2),若不是每次都重采样，那么代码第54行就要做相应修改，把w(j)加上去        
   #生成c
   c=[0]*n
   c[1]=w[1]
   for j in range(1,100):
       c[j]=c[j-1]+w[j]
        
   #转盘子，生成随机数，看落在哪个区间
   #首先我们要重采样n个粒子，粒子数要与之前相同
   for j in range(100):  
       a=random.uniform(0,1)#均匀分布
       for k in range(n):
            if a < c[k]:
                xnew[j]=xold[k]
                break#%%%%一定要break，否则重采样粒子会被最后一个粒子覆盖，具体见新的第十讲
   #把每一步的后验概率期望赋值给xplus
   xplus[i]=sum(xnew)/n     
   #重采样完毕
   #把新的粒子赋值给xold，为下一步递推做准备
   xold=xnew
   #权重都设为1/n
   for j in range(n):
       w[j]=1/n
    
  


plt.plot(t,x,'r',t,xplus,'b')