为了简化，我们给出已经推导出的线性状态空间模型的粒子滤波器算法。粒子滤波器是针对以下状态空间模型推导出来的：
$$ \begin{aligned} & x _k=A x _{k-1}+B u _{k-1}+ w _{k-1} \\ & y _k=C x _k+ v _k \end{aligned}\qquad(1) $$
其中 $x _k \in R ^n$ 是离散时间步长 $k$ 的状态向量，$u _{k-1} \in R ^{m_1}$ 是时间步长 $k-1$ 的控制输入向量，$w _{k-1} \in R ^{m_2}$ 是时间步长 $k-1$ 处的过程扰动向量（过程噪声向量），$y _k \in R ^r$ 是时间步长 $k$ 处观测到的输出矢量，$v _k \in R ^r$ 是离散时间步长 $k$ 处的测量噪声向量，$A，B$和$C$是系统矩阵。
假设过程扰动向量 $w _k$ 服从正态分布，具有零均值和规定的协方差矩阵，即
$$ w _k \sim N (0, Q)\qquad(2) $$
其中 $Q$ 是过程扰动向量的协方差矩阵。另外，假设测量噪声向量 $v _k$ 服从正态分布，具有零均值和规定的协方差矩阵，即
$$ v _k \sim N (0, R)\qquad(3) $$
其中 $R$ 是测量噪声向量的协方差矩阵。状态转移密度是以下正态分布的密度
$$ N \left(A x _{k-1}+B u _{k-1}, Q\right)\qquad(4) $$
此外，我们还证明了测量密度（测量概率密度函数），表示为 $p\left( y _k \mid x _k\right)$，是一个正态分布，其平均值为 $C x _k$，协方差矩阵等于测量噪声向量 $v _k$ 的协方差矩阵。也就是说，测量密度是以下正态分布的密度
$$ N \left(C x _k, R\right)\qquad(5) $$
为了实现粒子滤波器，我们需要从状态转换概率 (4) 中抽取 $x _k$ 的样本。有两种方法可用于生成这些样本。第一种方法（我们在 Python 实现中使用）是从 (2) 中给出的分布中抽取过程扰动向量的随机样本。
在每个离散时间点 $k$，粒子滤波器计算以下一组粒子
$$ \left\{\left( x _k^{(i)}, w_k^{(i)}\right) \mid i=1,2,3, \ldots, N\right\}\qquad(6) $$
索引为 $i$ 的粒子由元组 $\left( x _k^{(i)}, w_k^{(i)}\right)$ 组成，其中 $x _k^{(i)}$ 是状态样本，$w_k^{(i)}$ 是重要性权重。粒子集近似后验密度 $p\left(x_k \mid y _{0: k}, u _{0: k-1}\right)$，如下所示
$$ p\left( x _k \mid y _{0: k}, u {0: k-1}\right) \approx \sum{i=1}^N w_k^{(i)} \delta\left( x _k- x _k^{(i)}\right)\qquad(7) $$
粒子滤波算法的说明：对于初始粒子集
$$ \left\{\left( x _0^{(i)}, w_0^{(i)}\right) \mid i=1,2,3, \ldots, N\right\} $$
import time
import numpy as np
import matplotlib.pyplot as plt
from scipy.stats import multivariate_normal
def systematicResampling(weightArray):
N=len(weightArray)
cValues=[]
cValues.append(weightArray[0])
for i in range(N-1):
cValues.append(cValues[i]+weightArray[i+1])
startingPoint=np.random.uniform(low=0.0, high=1/(N))
resampledIndex=[]
for j in range(N):
currentPoint=startingPoint+(1/N)*(j)
s=0
while (currentPoint>cValues[s]):
s=s+1
resampledIndex.append(s)
return resampledIndex
meanProcess=np.array([0,0])
covarianceProcess=np.array([[0.002, 0],[0, 0.002]])
meanNoise=np.array([0])
covarianceNoise=np.array([[0.001]])
processDistribution=multivariate_normal(mean=meanProcess,cov=covarianceProcess)
noiseDistribution=multivariate_normal(mean=meanNoise,cov=covarianceNoise)
m=5
ks=200
kd=30
Ac=np.array([[0,1],[-ks/m, -kd/m]])
Cc=np.array([[1,0]])
Bc=np.array([[0],[1/m]])
h=0.01
A=np.linalg.inv(np.eye(2)-h*Ac)
B=h*np.matmul(A,Bc)
C=Cc
simTime=1000
x0=np.array([[0.1],[0.01]])
stateDim,tmp11=x0.shape
controlInput=100*np.ones((1,simTime))
stateTrajectory=np.zeros(shape=(stateDim,simTime+1))
output=np.zeros(shape=(1,simTime))
stateTrajectory[:,[0]]=x0
for i in range(simTime):
stateTrajectory[:,[i+1]]=np.matmul(A,stateTrajectory[:,[i]])+np.matmul(B,controlInput[:,[i]])+processDistribution.rvs(size=1).reshape(stateDim,1)
output[:,[i]]=np.matmul(C,stateTrajectory[:,[i]])+noiseDistribution.rvs(size=1).reshape(1,1)
x0Guess=x0+np.array([[0.7],[-0.6]])
pointsX, pointsY = np.mgrid[x0Guess[0,0]-0.8:x0Guess[0,0]+0.8:0.1, x0Guess[1,0]-0.5:x0Guess[1,0]+0.5:0.1]
xVec=pointsX.reshape((-1, 1), order="C")
yVec=pointsY.reshape((-1, 1), order="C")
states=np.hstack((xVec,yVec)).transpose()
dim1,numberParticle=states.shape
weights=(1/numberParticle)*np.ones((1,numberParticle))
numberIterations=1000
stateList=[]
stateList.append(states)
weightList=[]
weightList.append(weights)
for i in range(numberIterations):
controlInputBatch=controlInput[0,i]*np.ones((1,numberParticle))
newStates=np.matmul(A,states)+np.matmul(B,controlInputBatch)+processDistribution.rvs(size=numberParticle).transpose()
newWeights=np.zeros(shape=(1,numberParticle))
for j in range(numberParticle):
meanDis=np.matmul(C,newStates[:,[j]])
distributionO=multivariate_normal(mean=meanDis[0],cov=covarianceNoise)
newWeights[:,j]=distributionO.pdf(output[:,i])*weights[:,[j]]
weightsStandardized=newWeights/(newWeights.sum())
tmp1=[val**2 for val in weightsStandardized]
Neff=1/(np.array(tmp1).sum())
if Neff<(numberParticle//3):
resampledStateIndex=np.random.choice(np.arange(numberParticle), numberParticle, p=weightsStandardized[0,:])
newStates=newStates[:,resampledStateIndex]
weightsStandardized=(1/numberParticle)*np.ones((1,numberParticle))
states=newStates
weights=weightsStandardized
stateList.append(states)
weightList.append(weights)
meanStateSequence=np.zeros(shape=(stateDim,numberIterations))
for i in range(numberIterations):
meanState=np.zeros(shape=(stateDim,1))
for j in range(numberParticle):
meanState=meanState+weightList[i][:,j]*stateList[i][:,j].reshape(2,1)
meanStateSequence[:,[i]]=meanState