Python FilterPy包：贝叶斯滤波器库
安装filterpy
pip install filterpy
注意:运行filterpy包还需要numpy，scipy和matplotlib。
测试filterpy版本：
import filterpy
filterpy.__version__ #'1.4.5'
常用的模块名：
一、filterpy.kalman包含很多滤波算法，例如：
1.线性滤波算法：KalmanFilter、Saver、FixedLagSmoother、SquareRootKalmanFilter、InformationFilter、FadingKalmanFilter、MMAE Filter Bank、IMM Estimator
2.非线性滤波算法：ExtendedKalmanFilter、UnscentedKalmanFilter、EnsembleKalmanFilter
二、filterpy.common
包含支持filter类和函数的各种有用函数。最有用的是计算过程噪声矩阵Q的函数。它还实现了线性微分方程式的Van Loan离散化。
三、filterpy.stats
包含对卡尔曼滤波有用的统计函数，例如多元高斯乘法，计算对数似然，NESS和马氏距离，以及用于绘制多元高斯CDF，PDF和协方差椭圆。
四、filterpy.monte_carlo
计算马尔可夫链蒙特卡洛（MCMC），主要用于粒子滤波。
五、filterpy.discrete_bayes
用于执行离散贝叶斯滤波。
六、filterpy.gh
这些功能是为各种常用滤波器的g和h参数提供设置。
七、filterpy.memory
八、filterpy.hinfinity
九、filterpy.leastsq
示例：kalman filter
参数介绍：
1、输入参数
x : ndarray (dim_x, 1), default = [0,0,0…0] 表示滤波器需要估计的状态向量
P : ndarray (dim_x, dim_x), default eye(dim_x) 表示协方差矩阵
Q : ndarray (dim_x, dim_x), default eye(dim_x) 表示过程噪声（系统噪声）
R : ndarray (dim_z, dim_z), default eye(dim_x) 表示量测噪声
H : ndarray (dim_z, dim_x) 表示量测方程
F : ndarray (dim_x, dim_x) 表示状态转移方程
B : ndarray (dim_x, dim_u), default 0 表示控制转移矩阵
2、可选参数
alpha : float
Assign a value > 1.0 to turn this into a fading memory filter.
3.可参看参数
K : ndarray 卡尔曼增益
y : ndarray 残差，实际量测和当前估计状态映射到量测空间的量测之间的差异 =(z - Hx)
S : ndarray 系统不确定性映射到测量空间 =HPH’ + R 。
likelihood : float 上次测量更新的似然。
log_likelihood : float 上次测量更新的对数似然。
卡尔曼滤波代码：
from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noise
#filter
kf = KalmanFilter(dim_x=2, dim_z=1) #dim_x:隐状态大小，dim_z:量测大小
#定义参数
kf.x = x #初始状态[位置,速度]
kf.F = F #状态转移矩阵
kf.H = np.array([[1.,0.]]) #量测矩阵
kf.P = P #初始状态协方差
kf.R = R #量测噪声
kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=1e-2) #过程（系统）噪声
z = get_sensor_reading() #输入量测
f.predict() #预测
f.update(z) #更新
do_something_with_estimate (f.x) #f.x更新后的状态
完整卡尔曼滤波示例代码
import numpy as np
import matplotlib.pyplot as plt
from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noise
np.random.seed(2)
def demo():
dt = 0.1
F = np.array([[1, dt], [0, 1]])
Q = 1e-2*np.array([[1/4*dt**4, 1/2*dt**3], [1/2*dt**3, dt**2]])
R = 2.
itr = 100
real_state = []
x = np.array([10,5]).reshape(2,1)
for i in range(itr):
real_state.append(x[0,0])
x = np.dot(F,x)+np.random.multivariate_normal(mean=(0,0),cov=Q).reshape(2,1)
measurements = [x+np.random.normal(0,R) for x in real_state]
# initialization
P = np.array([[10,5],[5,10]])
x=np.random.multivariate_normal(mean=(10,5),cov=P).reshape(2,1)
#filter
kf = KalmanFilter(dim_x=2, dim_z=1) #dim_x:隐状态大小，dim_z:量测大小
#定义参数
kf.x = x #初始状态[位置,速度]
kf.F = F #状态转移矩阵
kf.H = np.array([[1.,0.]]) #量测矩阵
kf.P = P #初始状态协方差
kf.R = R #量测噪声
kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=1e-2) #过程（系统）噪声
filter_result=list()
filter_result.append(x)
for i in range(1,itr):
z = measurements[i]
kf.predict()
kf.update(z)
filter_result.append(kf.x)
filter_result=np.squeeze(np.array(filter_result))
return measurements,real_state,filter_result
def plot_result(measurements,real_state,filter_result):
plt.figure(figsize=(8,4))
plt.plot(range(1,len(measurements)), measurements[1:], label = 'Measurements')
plt.plot(range(1,len(real_state)), real_state[1:], label = 'Real statement' )
plt.plot(range(1,len(filter_result)), np.array(filter_result)[1:,0], label = 'Kalman Filter')
plt.legend()
plt.xlabel('Time',fontsize=14)
plt.ylabel('velocity [m]',fontsize=14)
plt.show()
plt.figure(figsize=(8,4))
plt.axhline(5, label='Real statement') #, label='$GT_x(real)$'
plt.plot(range(1,len(filter_result)), np.array(filter_result)[1:,1], label = 'Kalman Filter')
plt.legend()
plt.xlabel('Time',fontsize=14)
plt.ylabel('velocity [m]',fontsize=14)
plt.show()
if __name__ == '__main__':
measurements,real_state,filter_result=demo()
plot_result(measurements,real_state,filter_result)
滤波后的结果如下图所示，图1表示目标位置的滤波结果，从图中可直观发现经过卡尔曼滤波后，消除了量测数据中的噪声，得到了更精确的目标位置；图1表示目标速度的滤波结果，从中可以发现经过卡尔曼滤波后，速度的估计越来越精确且逐渐收敛到了真实的速度
v
=
5
m
/
s
v=5m/s
v
=
5
m
/
s
。
图1：目标位置的滤波结果
图2：目标速度的滤波结果
参考资料
1.
filterpy
2.
滤波系列（一）卡尔曼滤波算法（KF）：详细数学推导
3.
滤波系列（一）卡尔曼滤波算法（KF）：应用