07-16 08:36 已编辑上海海事大学单片机发布于香港

关注

滤波器入门-03（离散贝叶斯滤波器）

回顾：在滤波器入门-02中我们对g-h滤波器的相关概念进行了明确，并进行了几个小实验，加深了我们对其特性的理解。本小节我们将学习离散贝叶斯滤波器，Let's go.

Bayes' theorem is a formula to incorporate new information into existing information.

\begin{array}{l} update~knowledge = \parallel likelihood~of~new~knowledge \times prior~knowledge\parallel \\ posterior = \frac{{likelihood \times prior}}{{normalizationfactor}} \end{array}

$\parallel \cdot \parallel$ 表示归一化操作，上面两个公式都是贝叶斯定理思想的表现形式。

本节同样采用举例子的方式来帮助我们理解离散贝叶斯滤波器，假设我们有一个可以报告自己宠物狗位置和运动信息的传感器，我们的目的是尽可能利用它准确的估计出dog在房间的哪个位置，听起来似乎很有意思，让我们一步一步来。

首先离散化我们的房间，将其十等分，为便于编程，假设我们的房间是一个环形走廊，在特定位置有三个门。在初始情况下，dog在每个位置是等概率。

In Bayesian statistics this is called a prior. More completely, this is called the prior probability distribution. A probability distribution is a collection of all possible probabilities for an event.

import numpy as np
hallway = np.array([1, 1, 0, 0, 0, 0, 0, 0, 1, 0])#1代表该位置是门，0代表走廊
belief = np.array([1/10]*10)
print(belief)

输出初始先验概率分布如下：

[0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1]

Bayes treats this as a belief about a single event

belief : A measure of the strength of our knowledge.

alt

当传感器检测到门的时候更新概率分布，此时dog在三个门所在的位置是等概率的

alt

We are already comfortable assigning a probabilistic belief to the location of the dog; now we have to incorporate the additional uncertainty caused by the sensor noise. 但是传感器的检测可能存在误差（如果dog坐在门前抓挠自己，也许传感器不会检测到门，或者如果它没有面向走廊，传感器就会误读），我们不能完全信任传感器检测到的值。假设传感器检测正确的概率是错误的3倍，那么按照此倍率在有门的地方缩放后的初始的概率分布结果如下：

belief: [0.3 0.3 0.1 0.1 0.1 0.1 0.1 0.1 0.3 0.1]
sum = 1.6000000000000003

从结果可以很明显的看出，按照测量更新后的分布不符合概率分布的性质（各事件概率和为一，因此需要对其进行归一化处理）,代码和更新后的概率分布如下

from filterpy.discrete_bayes import normalize

def scaled_update(hall, belief, z, z_prob): 
    scale = z_prob / (1. - z_prob)#z_prob为测量准确的概率
    likelihood = np.ones(len(hall))
    likelihood[hall==z] *= scale
    return normalize(likelihood * belief)

belief = np.array([0.1] * 10)
scaled_update(hallway, belief, z=1, z_prob=.75)

alt

This result is called the posterior, which is short for posterior probability distribution. All this means is a probability distribution after incorporating the measurement information.

重新回顾下公式

posterior = \frac{{likelihood \times prior}}{{normalization}}

When we talk about the filter's output we typically call the state after performing the prediction the prior or prediction, and we call the state after the update either the posterior or the estimated state.

总体思路就是依据传感器测量得到的值计算likelihood（似然），更新先验概率后归一化得到后验概率。上面的代码还不够通用，将计算似然概率的步骤分离出来，更新后的代码如下：

from filterpy.discrete_bayes import update

def update(likelihood, prior):
    return normalize(likelihood * prior)
  
def lh_hallway(hall, z, z_prob):
    """ compute likelihood that a measurement matches
    positions in the hallway."""
    
    try:
        scale = z_prob / (1. - z_prob)
    except ZeroDivisionError:
        scale = 1e8

    likelihood = np.ones(len(hall))
    likelihood[hall==z] *= scale
    return likelihood

belief = np.array([0.1] * 10)
likelihood = lh_hallway(hallway, z=1, z_prob=.75)
update(likelihood, belief)

仅有位置信息我们还无法做到对dog位置的准确估计，试想当传感器报告的信息为“门-向右-门”，那么我们就可以大概率得到dog目前位于1号门位置处了，接下来，融入传感器检测到的运动信息，用来作为本系统（dog）的过程模型预测下一时刻的状态(位置)，同样考虑到误差的存在，即在与预测步骤中添加不确定性，在此例中表现为在预测dog向右移动的n个单位的概率为0.8，移动n-1和n+1个单位的概率为0.1。

def predict_move(belief, move, p_under, p_correct, p_over):
    n = len(belief)
    prior = np.zeros(n)
    for i in range(n):
        prior[i] = (
            belief[(i-move) % n]   * p_correct +
            belief[(i-move-1) % n] * p_over +
            belief[(i-move+1) % n] * p_under)      
    return prior

belief = np.array([1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
predict_beliefs = []
    
for i in range(100):
    belief = predict_move(belief, 1, .1, .8, .1)
    predict_beliefs.append(belief)

在没有使用测量位置更新先验，仅执行预测步骤的概率迭代变化过程可视化如下所示：

alt

可以看到，在仅使用预测，不使用测量得到位置的情况下我们最终失去了所有的信息。

接下来，泛化代码，dog每次可能会移动多个单位，我们使用卷积（乘积之和）替代上述含有误差的预测流程。

\begin{array}{l} (f * g)(t) = \int_0^t {f(\tau )g(t - \tau )d\tau } ~~~~continuous~functions\\ (f * g)[t] = \sum\limits_{\tau = 0}^t {f[\tau ]g[t - \tau ]} ~~~~discrete~functions \end{array}

代码如下：

def predict_move_convolution(pdf, offset, kernel):
    N = len(pdf)
    kN = len(kernel)
    width = int((kN - 1) / 2)

    prior = np.zeros(N)
    for i in range(N):
        for k in range (kN):
            index = (i + (width-k) - offset) % N
            prior[i] += pdf[index] * kernel[k]
    return prior

This illustrates the algorithm, but it runs very slow.SciPy provides a convolution routine convolve() in the ndimage.filters module. 调库预测代码如下：

from filterpy.discrete_bayes import predict

belief = [.05, .05, .05, .05, .55, .05, .05, .05, .05, .05]
prior = predict(belief, offset=1, kernel=[.1, .8, .1])

计算位置4和6的值： (0.1×0.05)+(0.8×0.05)+(0.1×0.55)=0.1

计算位置5的值： (0.1×0.05)+(0.8×0.55)+(0.1×0.05)=0.45

alt

当运动单位大于1（此处设置为3），且考虑过冲相比下冲具有更高的权重，采用不对称的卷积核，调整后代码和输出概率分布可视化如下：

prior = predict(belief, offset=3, kernel=[.05, .05, .6, .2, .1])

alt

现在我们建立的过程模型如下,在此例中式中 ${{\bf{x}}_k}$ 为一维向量

{{\bf{\bar x}}_{k + 1}} = {{\bf{x}}_k} * {f_{\bf{x}}}( \cdot )

最后就是将预测和更新的步骤集成，得到离散贝叶斯滤波器，它的整体流程和g-h滤波器是一致的。

alt

\begin{array}{l} \bar x = x * {f_x}( \cdot )~~~~~Predict~Step\\ x = \parallelℒ \cdot \bar x\parallel ~~~~~Update~Step \end{array}

伪代码如下：

Initialization

Initialize our belief in the state

Predict

Based on the system behavior, predict state for the next time step
Adjust belief to account for the uncertainty in prediction

Update

Get a measurement and associated belief about its accuracy
Compute how likely it is the measurement matches each state
Update state belief with this likelihood

代码如下

def discrete_bayes_sim(prior, kernel, measurements, z_prob, hallway):
    posterior = np.array([.1]*10)
    priors, posteriors = [], []
    for i, z in enumerate(measurements):
        prior = predict(posterior, 1, kernel)
        priors.append(prior)

        likelihood = lh_hallway(hallway, z, z_prob)
        posterior = update(likelihood, prior)
        posteriors.append(posterior)
    return priors, posteriors

接下来模拟下该滤波器对错误测量的影响

hallway = np.array([1, 0, 1, 0, 0]*2)
kernel = (.1, .8, .1)
prior = np.array([.1] * 10)
zs = [1, 0, 1, 0, 0, 1, 1, 1, 0, 0]#第7个测量出错
z_prob = 0.75
priors, posteriors = discrete_bayes_sim(prior, kernel, zs, z_prob, hallway)

alt

从结果可以看到尽管第7次测量出错对我们的后验概率分布造成一些影响，但在下一轮正确的测量下，我们依然可以dog的位置抱有很高的brief。

目前此算法的一些局限性：

The first problem is scaling.We have not covered the multidimensional case, but instead of an array we use a multidimensional grid to store the probabilities at each discrete location.Each update() and predict() step requires updating all values in the grid, so a simple four variable problem would require O( $n^4$ ) running time per time step.
The filter is discrete, but we live in a continuous world.A 100 meter hallway requires 10,000 positions to model the hallway to 1cm accuracy.A 100x100 m2 courtyard requires 100,000,000 bins to get 1cm accuracy.It gets exponentially worse as we add dimensions.
*The filter is multimodal.*但在实际生活中我们需要输出一个位置，而不是多个可能性。
*It requires a measurement of the change in state.*在预测步骤需要依靠运动传感器测量得到的信息进行过程传播。

We have implemented a form of a Bayesian filter.We have learned how to start with no information and derive information from noisy sensors.Even though the sensors in this chapter are very noisy (most sensors are more than 80% accurate, for example) we quickly converge on the most likely position for our dog.We have learned how the predict step always degrades our knowledge, but the addition of another measurement, even when it might have noise in it, improves our knowledge, allowing us to converge on the most likely result.

If you can understand this chapter you will be able to understand and implement Kalman filters.

However, if you grasp the fundamental insight - multiplying probabilities when we measure, and shifting probabilities when we update leads to a converging solution - then after learning a bit of math you are ready to implement a Kalman filter.

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