【卡尔曼滤波器-Python】The g-h filter

【卡尔曼滤波器-Python】The g-h filter

The g-h filter是kalman的基本思想。通过经验值来改进测量值，达到滤波的效果。举个例子，假如有一个人有一天的重量是160斤，并且接下来每天的体重都会增加一斤，但是我们通过体重计测得的数据分别如下所示

[158.0, 164.2, 160.3, 159.9, 162.1, 164.6,169.6, 167.4, 166.4, 171.0, 171.2, 172.6]

滤波的过程如下

1. 根据线性增长确定真实值
2. 计算残差，真实值与测量值
3. 根据残差的大小，在h因子的作用下确定下次增长率
4. 根据残差的大小，在g因子的作用下确定估计值，即滤波后的结果

测试代码如下

from gh_internal import plot_g_h_results
import book_plots
import numpy as np


def g_h_filter(data,x0,dx,g,h,dt=1.,pred=None):
    x=x0
    results=[]
    for z in data:
        x_est=x+(dx*dt)
        dx=dx
        if pred is not None:
            pred.append(x_est)
        residual=z-x_est
        dx=dx+h*(residual)/dt
        x=x_est+g*residual
        results.append(x)
    return np.array(results)

weights=np.array([158.0, 164.2, 160.3, 159.9, 162.1, 164.6,
                  169.6, 167.4, 166.4, 171.0, 171.2, 172.6])
book_plots.plot_track([0, 11], [160, 172], label='Actual weight')
data = g_h_filter(data=weights, x0=160, dx=1, g=3./10, h=1./3, dt=1.)
plot_g_h_results(weights, data);

[1] Kalman_and_Bayesian_FIlters_in_Python
[2] https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python