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=5m/s$。

图1：目标位置的滤波结果

图2：目标速度的滤波结果

参考资料

1.filterpy
2.滤波系列（一）卡尔曼滤波算法（KF）：详细数学推导
3.滤波系列（一）卡尔曼滤波算法（KF）：应用