FilterPy项目中的GH滤波器算法详解

什么是GH滤波器

GH滤波器是一种简单但有效的状态估计算法，属于α-β滤波器的变种。它通过两个增益参数g和h来平衡当前测量值与预测值之间的权重，适用于跟踪系统状态及其变化率(速度)的场景。

GH滤波器在FilterPy中的实现

FilterPy项目提供了三种GH滤波器的实现：

1. GHFilterOrder：支持0阶、1阶和2阶的通用GH滤波器
2. GHFilter：标准的1阶GH滤波器实现
3. GHKFilter：带有二阶导数(加速度)的GHK滤波器

GHFilterOrder类详解

GHFilterOrder是一个灵活的GH滤波器实现，支持0到2阶的系统模型：

class GHFilterOrder(object):
    def __init__(self, x0, dt, order, g, h=None, k=None):
        # 初始化代码
        pass
    
    def update(self, z, g=None, h=None, k=None):
        # 更新状态
        pass

参数说明

• x0: 初始状态值，可以是标量或数组
• dt: 时间步长
• order: 滤波器阶数(0,1或2)
• g: 位置增益参数
• h: 速度增益参数(1阶和2阶需要)
• k: 加速度增益参数(仅2阶需要)

工作原理

根据阶数不同，更新方程也不同：

• 0阶：仅跟踪位置

  y = z - x
  x += g * y
  

• 1阶：跟踪位置和速度

  y = z - (x + dx*dt)
  x += dx*dt + g*y
  dx += h*y/dt
  

• 2阶：跟踪位置、速度和加速度

  y = z - (x + dx*dt + 0.5*ddx*dt²)
  x += dx*dt + 0.5*ddx*dt² + g*y
  dx += ddx*dt + h*y/dt
  ddx += 2*k*y/dt²
  

GHFilter类详解

GHFilter是标准的1阶GH滤波器实现，接口更简单：

class GHFilter(object):
    def __init__(self, x, dx, dt, g, h):
        # 初始化代码
        pass
    
    def update(self, z, g=None, h=None):
        # 更新状态
        pass
    
    def batch_filter(self, data, save_predictions=False, saver=None):
        # 批量处理数据
        pass

核心方法

1. update()：单次更新滤波器状态

   • 预测步骤：x_pred = x + dx*dt
   • 更新步骤：使用测量值z修正预测

2. batch_filter()：批量处理测量数据

   • 适用于离线处理历史数据
   • 可选择保存预测结果

GHKFilter类详解

GHKFilter在GHFilter基础上增加了对加速度的跟踪：

class GHKFilter(object):
    def __init__(self, x, dx, ddx, dt, g, h, k):
        # 初始化代码
        pass
    
    def update(self, z, g=None, h=None, k=None):
        # 更新状态
        pass

更新方程考虑了二阶导数：

y = z - (x + dx*dt + 0.5*ddx*dt²)
x += dx*dt + 0.5*ddx*dt² + g*y
dx += ddx*dt + h*y/dt
ddx += 2*k*y/dt²

如何选择滤波器参数

选择合适的g、h、k参数是GH滤波器设计的关键：

1. g参数：控制测量值对位置估计的影响

   • 值越大，滤波器对测量变化越敏感
   • 值越小，滤波器越平滑但响应延迟越大

2. h参数：控制测量值对速度估计的影响

   • 影响速度跟踪的响应性

3. k参数：控制测量值对加速度估计的影响

   • 仅用于2阶滤波器

应用场景

GH滤波器适用于以下场景：

1. 目标跟踪系统
2. 传感器数据平滑
3. 实时系统状态估计
4. 计算资源有限的嵌入式系统

性能分析

FilterPy中的GH滤波器实现提供了VRF(方差缩减因子)计算方法：

# 计算预测步骤的VRF
VRF_prediction()

# 计算状态变量的VRF
VRF()

VRF可以帮助分析滤波器的噪声抑制性能，值越小表示滤波效果越好。

总结

FilterPy项目提供的GH滤波器实现具有以下特点：

1. 支持从简单到复杂的不同阶数模型
2. 接口设计清晰易用
3. 提供批量处理功能
4. 包含性能分析工具
5. 计算高效，适合实时系统

对于需要简单有效的状态估计算法的应用，GH滤波器是一个很好的选择，而FilterPy提供了高质量的实现参考。