g-h滤波--卡尔曼滤波(一)

本文是对Kalman-and-Bayesian-Filters-in-Python书里内容的概括总结，原文请参考：
Kalman-and-Bayesian-Filters-in-Python

一. 数据的取值

我们现在有两台体重秤A和B，但体重秤每次的测量都是不准确的，假设两次测量的体重分别为160和170斤，我该怎么选取体重值呢？

• 相信第一次的值160
• 相信第二次的值170
• 选一个值比两个值都大
• 选一个值比两个都小
• 选一个值在两者之间

前两个选择是可以的，但我们没有理由偏向于一个。为什么我们选择相信A而不是B？我们没有理由相信这一点。第三和第四选择是不合理的，体重秤当然不是很准确，但是根本没有理由选择超出两者均测量范围的数字。最后的选择是唯一合理的选择。如果两个刻度都不正确，并且结果可能超过我的实际体重，也可能低于我的实际体重，那么答案通常是在A和B之间。

假设我们的两台体重秤有它各自的误差范围,A的误差范围是3，B的误差范围是9，如下图所示：

图1

我们的直觉告诉我们，选择两者重叠的部分是更合理的。两台传感器，即使其中的一台精确度不如另一台，两台也能提供更可靠的数据。我们不应该丢弃任何数据！！！

如果我只有一个体重秤，但可以多次称重自己该怎么办？ 我们得出的结论是，如果我们有两个精度相等的量表，则应平均其测量结果。 如果我用一个秤称量自己10,000次会怎样？ 我们会得到一个既不会太大也不会太小的数据。 不难证明大量权重的平均值将非常接近实际权重。就像下图这样：

图2

我们取得的值看起来不错，使用均值是合理的。

二. g-h模型

假设我们每隔一天使用这台体重秤测试一次自己的体重，如果得到的数据是158.0, 164.2, 160.3, 159.9, 162.1, 164.6, 169.6, 167.4, 166.4, 171.0 。这些数据参差不齐，是测量的误差吗？但这些数字看起来又好像在增加，我们先采用平均值的方式，如下图

图3

使用平均值不合理，实际上我们不能画一条水平线在所有测量的误差内。
现在我们假设体重一直在增加，通过工具，采用最小二乘法画一条线，如图：

图4

这条线看起来好多了，覆盖了所有测量，更合理。但事实一定是这样吗？我就不能一周增加13斤(最小值158，最大值171)？这可能吗？

假设我们就认为我每天增加1斤的体重(滤波器模型)，第一次测量158，那么第二天我的体重就是159，让我们看看体重秤的第二次读数164.2。我们遇到了一个问题，我们的预测和我们的测量不一致。 但是，这就是我们所期望的， 如果预测始终与测量完全相同，则它将无法向过滤器添加任何信息。 同样，由于我们的预测是完美的，因此没有理由进行测量。我们需要将预测和测量以某种方式混合。

混合两个值-听起来很像前面的两个体重秤的问题。我们不要丢弃任何信息，预测值和测量值都不能丢弃。使用与以前相同的推理，我们可以看到，唯一有意义的是在预测和测量之间选择一个数字。估计值要介于测量和预测之间吗？也许可以，总的来说，我们的预测值比测量值或多或少是准确的(因为整体趋势上是上升的)。我们假设估计值是这样取得的：

estimate = prediction + 4/10 x (measurement - prediction)

我们使预测模型的初始值为160，编码如下：

from kf_book.book_plots import figsize
import matplotlib.pyplot as plt

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]

time_step = 1.0  # day
scale_factor = 4.0/10

def predict_using_gain_guess(estimated_weight, gain_rate, do_print=False):     
    # storage for the filtered results
    estimates, predictions = [estimated_weight], []

    # most filter literature uses 'z' for measurements
    for z in weights: 
        # predict new position
        predicted_weight = estimated_weight + gain_rate * time_step

        # update filter 
        estimated_weight = predicted_weight + scale_factor * (z - predicted_weight)

        # save and log
        estimates.append(estimated_weight)
        predictions.append(predicted_weight)
        if do_print:
            gh.print_results(estimates, predicted_weight, estimated_weight)

    return estimates, predictions

initial_estimate = 160.
estimates, predictions = predict_using_gain_guess(
    estimated_weight=initial_estimate, gain_rate=1, do_print=True)

执行输出得到

previous estimate: 160.00, prediction: 161.00, estimate 159.80
previous estimate: 159.80, prediction: 160.80, estimate 162.16
previous estimate: 162.16, prediction: 163.16, estimate 162.02
previous estimate: 162.02, prediction: 163.02, estimate 161.77
previous estimate: 161.77, prediction: 162.77, estimate 162.50
previous estimate: 162.50, prediction: 163.50, estimate 163.94
previous estimate: 163.94, prediction: 164.94, estimate 166.80
previous estimate: 166.80, prediction: 167.80, estimate 167.64
previous estimate: 167.64, prediction: 168.64, estimate 167.75
previous estimate: 167.75, prediction: 168.75, estimate 169.65
previous estimate: 169.65, prediction: 170.65, estimate 170.87
previous estimate: 170.87, prediction: 171.87, estimate 172.16

每次用估计的值进行预测，然后将预测的值与测量值按比例混合，得到新的估计值，不断的迭代。请仔细查看代码，确保清楚每一个值的计算过程。

我们将数据打印到图表中，如下：

图5

我们蓝色曲线模拟的效果非常好，它比单一的测量和预测值都更符合实际。也许我们的预测模型刚好符合实际，那如果我们的预测模型是每天减少1斤呢？

e, p = predict_using_gain_guess(initial_estimate, -1.)
#打印
gh.plot_gh_results(weights, estimates, predictions, [160, 172]) 

如下图所示：

图6
滤波器没有上次好，但是！滤波器在调整自己，只不过调整的速度有点慢。如果体重的增加或者减少也是从预测和测量中计算出来呢？我么现在第二天的预测值是
(160+1)+4 /10*(158-161) = 159.8
我们的测量值是164.2，增加了4.4(164.2-159.8=4.4)而不是1，我么是否可以利用这个信息呢？ 我们不要丢弃任何信息，我们得预测的增量是1，而测量增量是4.4，我们是否可以混合这两个值？这和前面的问题是一样的。我们假设增加值是这样的：

new gain = old gain + 1/3 (measurement - predicted) / day

重新编码如下：

weight = 160.  # initial guess
gain_rate = -1.0  # initial guess

time_step = 1.
weight_scale = 4./10
gain_scale = 1./3
estimates = [weight]
predictions = []

for z in weights:
    # prediction step
    weight = weight + gain_rate*time_step
    gain_rate = gain_rate
    predictions.append(weight)
    
    # update step    
    residual = z - weight
    
    gain_rate = gain_rate + gain_scale   * (residual/time_step)
    weight    = weight    + weight_scale * residual
  
    estimates.append(weight)
gh.plot_gh_results(weights, estimates, predictions, [160, 172])

打印数据如下：

图7
Great! 这就是g-h滤波器 (这里4/10就是g, 3/10就是h)

该滤波器是包括Kalman滤波器在内的众多滤波器的基础。换句话说，卡尔曼滤波器是g-h滤波器的一种形式。总结一下该算法(这里采用原文)：

Initialization

1. Initialize the state of the filter
2. Initialize our belief in the state

Predict

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

Update

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

图8

三. g,h的选择

g的取值代表我们对预测和测量的信任程度。

图9

g越大滤波器更靠近测量值，越小越靠近预测值。

h反应的是滤波器的变化速度，这里不详细讲述，请参考原文。

图10

实际应用中对g和h的选择具体分析，这里不做讨论，相关请查看原文，重在理解g-h滤波模型的思想。

本节讲述了g-h滤波，是大部分滤波的核心思想，通过不断的在模型基础上预测和测量而得到估计值，笔者认为，滤波器的本质是得到可能的值，某一时刻的真实值是不知道的，但我们对滤波器的估计值有很高的信任度，就像文章开头我们使用两次测量的中间值一样。

实际的应用中单点的真实值意义不大，估计值很多情况能满足我们的需求。最后，不要丢弃任何数据。