g-h滤波

最新推荐文章于 2024-05-26 20:51:25 发布

jansenhaoo

最新推荐文章于 2024-05-26 20:51:25 发布

阅读量601

点赞数 1

分类专栏：滤波与优化文章标签：算法

原文链接：https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python

版权

滤波与优化专栏收录该内容

2 篇文章 0 订阅

订阅专栏

本文深入探讨了g-h滤波算法，强调了保持所有数据点的重要性以及如何在预测和测量之间找到平衡。通过不同参数设置的案例分析，展示了g和h对滤波效果的影响，如初始状态、噪声、加速度等因素。并以跟踪火车为例，说明如何调整g和h以适应实际问题，寻求稳定且准确的系统输出。

摘要由CSDN通过智能技术生成

g-h滤波

文章目录

g-h滤波

import numpy as np
#format the book作者自定义的包，用图的方式来更好展示原理
import book_format
import kf_book.gh_internal as gh
import kf_book.book_plots as book_plots
book_format.set_style()
book_plots.predict_update_chart()

g-h-filter_1_0

如下关键点需要强调.

多个数据点比一个数据点更准确，所以无论数据点多么不准确，都不要扔掉任何东西
始终在两个数据点之间选择一个数字，以创建更准确的估计值。
根据当前估计以及我们认为应该怎样变化，来预测下一次测量和变化率。
T然后选择新的估计值作为预测值和下一个测量值之间的一部分，并根据每个测量值的精度进行缩放。

算法可以表示为如下三步：

Initialization

Initialize the state of the filter
Initialize our belief in the state

Predict

Use system behavior to predict state at the next time step
Adjust belief to account for the uncertainty in prediction

Update

Get a measurement and associated belief about its accuracy
Compute residual between estimated state and measurement
New estimate is somewhere on the residual line

通用常因子g-h滤波

‘data’ ：量测数据.
‘x0’ ：状态初值
‘dx’ ：状态变化率，
‘g’ ： g-h滤波之g因子，更新x_est，代表预测值的权重
‘h’ ： g-h滤波之h因子，更新dx
‘dt’ ：步长

import matplotlib.pylab as pylab
from kf_book.gh_internal import plot_g_h_results
weights = [158.0, 164.2, 160.3, 159.9, 162.1, 164.6, 
           169.6, 167.4, 166.4, 171.0, 171.2, 172.6]

def g_h_filter(data,x0,dx,g,h,dt):
    x_est=x0
    results=[]#filter的估计结果
    for z in data:
        #predictions
        x_pred=x_est+(dx*dt)#预测值=上步估计+变化率*时间步
        dx=dx#dx状态变化率初值
        #update step
        residual =z-x_pred
        dx=dx+h*residual/dt#h预测值的 “增速” 更新
        x_est=x_pred+g*residual#g预测值和测量值的加权的权重
        results.append(x_est)
    return np.array(results)

book_plots.plot_track([0,11],[160,172],label='Actual weight')
data = g_h_filter(data=weights,x0=160,dx=1,g=0.6,h=2/3,dt=1)
gh.plot_g_h_results(weights,data)

g-h-filter_5_0

函数生成量测数据

from numpy.random import randn

def gen_data(x0,dx,count,noise_factor):#noise_factor是噪声标准差
    return [x0+dx*i+randn()*noise_factor for i in range(count)]

measurements=gen_data(0,1,50,1)
data=g_h_filter(data=measurements,x0=0,dx=1,dt=1,g=0.2,h=0.02)
gh.plot_g_h_results(measurements,data)

g-h-filter_7_0

上面的状态变化率初值dx设置为1，因为量测数据本身就是建立在一次函数的基础之上，所以可以看到一开始Filter对于观测值就追踪的很好

大dx初值

但是如果把dx设置的大于1，甚至很大会发生什么呢，这其中也可以看出算法的内在逻辑

measurements=gen_data(0,1,50,1)
data=g_h_filter(data=measurements,x0=0,dx=10,dt=1,g=0.2,h=0.02)#dx初始值为10，也就是x的初始预测增速为10
gh.plot_g_h_results(measurements,data)

g-h-filter_11_0

可以看到当状态变化率dx设置的很大dx=10时，在一开始Filter由于dx太大，导致估计值增加的太快，因此不能很好的跟踪测量值，而由于dx=dx+h*residual/dt，因此dx的更新会受到residual的影响，residual通过h将误差信息反馈给了dx，dx慢慢接近于合理的大小，因此Filter也就慢慢靠近量测值了

糟糕初值的影响

现在我用 gen_data生成100个点，起点为5，变化率为2，噪声标准差为10，然后用 g_h_filter进行处理， g=0.2 and h=0.02.

我们设置一个比较大的初值，即把滤波初值x0设置为200.

zs=gen_data(x0=5,dx=2,count=100,noise_factor=10)
data=g_h_filter(data=zs,x0=200,dx=2,dt=1,g=0.2,h=0.02)
gh.plot_g_h_results(measurements=zs,filtered_data=data)

g-h-filter_14_0

大噪声的影响

zs=gen_data(x0=5,dx=2,count=100,noise_factor=80)
data=g_h_filter(data=zs,x0=5,dx=2,dt=1,g=0.2,h=0.02)
gh.plot_g_h_results(measurements=zs,filtered_data=data)

g-h-filter_16_0

加速度的影响

def gen_data(x0,dx,count,noise_factor,accel=0):
    zs=[]
    for i in range(count):
        zs.append(x0+dx*i+randn()*noise_factor)
        dx+=accel#dx按照每步accel增长，即“加速度”
    return zs

prediction=[]
zs=gen_data(x0=10,dx=0,count=20,noise_factor=0,accel=2)
data=g_h_filter(data=zs,x0=10,dx=0,dt=1,g=0.2,h=0.02)

plt.xlim([0,20])
gh.plot_g_h_results(measurements=zs,filtered_data=data)

g-h-filter_18_0

变g

g是调节量测和预测所占比重的比例系数，调节g的大小看对Filter有什么样的影响

生成一段Noise_factor=50,dx=5的量测数据

np.random.seed(100)
zs=gen_data(x0=5,dx=5,count=50,noise_factor=50)

data1=g_h_filter(data=zs,x0=0,dx=5,dt=1,g=0.1,h=0.01)
data2=g_h_filter(data=zs,x0=0,dx=5,dt=1,g=0.4,h=0.01)
data3=g_h_filter(data=zs,x0=0,dx=5,dt=1,g=0.8,h=0.01)

with book_plots.figsize(y=4):
    book_plots.plot_measurements(zs,color='k')
    book_plots.plot_filter(data1,label='g=0.1',marker='s',c='C0')
    book_plots.plot_filter(data2,label='g=0.4',marker='v',c='C1')
    book_plots.plot_filter(data3,label='g=0.8',c='C2')
    plt.legend(loc=4)
    book_plots.set_limits([20,40],[50,250])

g-h-filter_20_0

可以看到g越大，估计结果越接近于量测值，g=0.8时估计数据几乎等同于量测数据

那是否g设定小一点，去除噪声的效果就会更好呢？这就意味着我们寄希望于预测而不是量测了

若某个点的跳动是信号本身的变化引起的而不是噪声呢

zs = [5, 6, 7, 8, 9, 10, 11, 12, 13, 14]
for i in range(50):
    zs.append(14)

data1 = g_h_filter(data=zs, x0=4., dx=1., dt=1., g=0.1, h=0.01)
data2 = g_h_filter(data=zs, x0=4., dx=1., dt=1., g=0.5, h=0.01)
data3 = g_h_filter(data=zs, x0=4., dx=1., dt=1., g=0.9, h=0.01)

book_plots.plot_measurements(zs)
book_plots.plot_filter(data1, label='g=0.1', marker='s', c='C0')
book_plots.plot_filter(data2, label='g=0.5', marker='v', c='C1')
book_plots.plot_filter(data3, label='g=0.9', c='C3')

plt.legend(loc=4)
plt.ylim([6, 20])

g-h-filter_22_0

可以看到很小的g确实把噪声过滤掉了，但是信号变化也跟不上了

需要找到合适的g、h值

不同的滤波器使用不同的方法来计算g和h

zs = [5,6,7,8,9,9,9,9,9,10,11,12,13,14,
      15,16,16,16,16,16,16,16,16,16,16,16]

data1 = g_h_filter(data=zs, x0=4., dx=1., dt=1., g=.302, h=.054)
data2 = g_h_filter(data=zs, x0=4., dx=1., dt=1., g=.546, h=.205)

book_plots.plot_measurements(zs)
book_plots.plot_filter(data2, label='g=0.546, h=0.205', marker='s', c='C0')
book_plots.plot_filter(data1, label='g=0.302, h=0.054', marker='v', c='C1')

plt.legend(loc=4)
plt.ylim([6, 18])

g-h-filter_24_0

变h

zs = np.linspace(0, 1, 50)

data1 = g_h_filter(data=zs, x0=0, dx=0., dt=1., g=.2, h=0.05)
data2 = g_h_filter(data=zs, x0=0, dx=2., dt=1., g=.2, h=0.05)
data3 = g_h_filter(data=zs, x0=0, dx=2., dt=1., g=.2, h=0.5)

book_plots.plot_measurements(zs)

book_plots.plot_filter(data1, label='dx=0, h=0.05', c='C0')
book_plots.plot_filter(data2, label='dx=2, h=0.05', marker='v', c='C1')
book_plots.plot_filter(data3, label='dx=2, h=0.5',  marker='s', c='C2')

plt.legend(loc=1);

g-h-filter_26_0

from ipywidgets import interact
from kf_book.book_plots import FloatSlider

zs1 = gen_data(x0=5, dx=5., count=100, noise_factor=50)

fig = None

def interactive_gh(x, dx, g, h):
    global fig
    if fig is not None: plt.close(fig)
    fig = plt.figure()
    data = g_h_filter(data=zs1, x0=x, dx=dx,dt=1,g=g, h=h)
    plt.scatter(range(len(zs1)), zs1, edgecolor='k', 
                facecolors='none', marker='o', lw=1)
    plt.plot(data, color='b')

interact(interactive_gh,           
         x=FloatSlider(value=0, min=-200, max=200), 
         dx=FloatSlider(value=5, min=-50, max=50), 
         g=FloatSlider(value=.1, min=.01, max=2, step=.02), 
         h=FloatSlider(value=.02, min=.0, max=.5, step=.01));
#通过下面四个划窗控制四个值的大小从而调整滤波效果

微信截图_20210819162828

g和h也不要设置的太小

zs = gen_data(x0=5, dx=-2, count=100, noise_factor=5)

data = g_h_filter(data=zs, x0=5., dx=2., dt=1., g=.005, h=0.001)
book_plots.plot_measurements(zs)
book_plots.plot_filter(data, label='filter')

plt.legend(loc=1)

g-h-filter_29_1

案例：跟踪火车

step1 构建火车量测值模型

pos=23000#设想火车位置为23m/s
vel=15#速度为15m/s

#假定火车匀速
def compute_new_position(pos,vel,dt=1.):
    return pos+(vel*dt)

#Add noise in to above equation(considering our assumption measurement error is 500m )
def measure_position(pos):
    return pos + random.randn()*500

生成100s的量测数据

# from numpy.random import randn
from numpy import random
def compute_new_position(pos,vel,dt=1.):
    return pos+(vel*dt)

def measure_position(pos):
    return pos + random.randn()*500

def gen_train_data(pos,vel,count):
#main func of generating data,input the guess 'pos','vel' and number of data(1 pear second,)
    zs=[]
    for t in range(count):
        pos=compute_new_position(pos,vel)
        zs.append(measure_position(pos))
    return np.asarray(zs)

pos,vel=23.*1000,10
zs=gen_train_data(pos,vel,100)
plt.plot(zs/1000)
book_plots.set_labels('Train Position','time(sec)','km')

g-h-filter_36_0

由于看到测量值很不好，因此我们可以给g-h滤波器设置一个小g来降低测量值的权重, 又因为火车速度不会突然变化，所以h可以设置成小值.

g=0.01,h=0.0001

zs = gen_train_data(pos=pos,vel=10.,count=100)
data=g_h_filter(zs,x0=pos,dx=15.,dt=1,g=0.01,h=0.0001)
gh.plot_g_h_results(zs/1000,data/1000,title='g=0.01,h=0.0001')

g-h-filter_38_0

g=0.2, h=0.0001

zs = gen_train_data(pos=pos, vel=15., count=100)
data = g_h_filter(data=zs, x0=pos, dx=15., dt=1., g=.2, h=0.0001)
gh.plot_g_h_results(zs/1000., data/1000., 'g=0.2, h=0.0001')

g-h-filter_40_0

上图可以看出g=0.2的时候，火车的位置和速度已经出现波动了，看起来不大，但是真实世界中火车还是不会这么运动的。因此，从经验上讲，我们要使得g<<0.2

再来选h

zs = gen_train_data(pos=pos, vel=15., count=100)
data = g_h_filter(data=zs, x0=pos, dx=15., dt=1., g=.01, h=0.1)
gh.plot_g_h_results(zs/1000., data/1000., 'g=0.01, h=0.1')

g-h-filter_42_0

带加速度

def gen_train_data_with_acc(pos,vel,count):
    zs=[]
    for t in range(count):
        pos=compute_new_position(pos,vel)
        vel+=0.2
        zs.append(measure_position(pos))
    return np.asarray(zs)

zs=gen_train_data_with_acc(pos=pos,vel=15.,count=100)
data=g_h_filter(data=zs,x0=pos,dx=15.,dt=1,g=0.01,h=0.001)
gh.plot_g_h_results(zs/1000,data/1000,'g=0.01,h=0.001')

g-h-filter_44_0

总结：

g表示预测值比重，用h来跟踪未被建模的速度变化，重要的是，我们要在反应的快速性和估计这种变化的精确性之间妥协，最终找到一个稳定的系统输出。如果火车匀速运动，那么我们就要设置一个非常小的h,这样就能避免滤波结果过多的受量测噪声的影响。不过有挑战的问题状态总是变化的。追求滤波器反应的快速性，必然要多受传感器噪声的影响。