__cplusplus在哪定义_让机器人知道自己在哪 alpha beta filter (g-h filter)

Formulation

假设你是一个机器人，在一条长长的马路的一头出发，期间经历了各种加速减速啥的。请问，你怎么知道自己在每一个时刻的具体位置呢？

你可能会说，这太简单了，你只要知道初始状态和两个加速度，你就能知道每个时刻的速度和位置了啊。问题当然没有那么简单，因为测量误差的存在，速度是不准的。简单点说，当机器人认为自己速度是0.2m/s的时候，它的实际速度很可能是0.21m每秒。这样的测量误差累计起来可能越来越大。

于是你想了个办法，给机器人装上个测距仪不就好了了，机器人实时测量自己离出发点的位置。但还是同样的问题，有测量就有误差。

既然测量误差不可避免，我们就要想想，我们应该多相信我们的测量呢？还有没有其他什么我们可以参考的信息吗？有，那就是预测。

开始之前，我们先定义清楚几个概念。

1. 首先机器人有两个测量的传感器，一个测量距离，一个测量速度。这里距离就是状态，而速度就是状态的变化。
2. 除了传感器之外，我们还可以对机器人下一位置进行预测。
3. 根据过去数据对状态的预测数据 和 当前的测量数据，我们对当前状态及状态的变化进行估计。

class Robot:
  def __init__(self, state=0, speed=0):
    self.state = state
    self.speed = speed
    # 机器人的实际位置
    self.actual_locations = [state]
  
  # 为了简化模型，这是一个只会做匀加速运动的机器人
  # 但其实gh-filter不需要是匀加速运动，根本不care你是什么运动
  def run(self, acc=0, t=0):
    if t <= 0: return
    for i in range(t):
      s1 = self.speed
      s2 = self.speed + acc
      self.state += (s1 + s2) / 2.0
      self.speed = s2;
      self.actual_locations.append(self.state)

我们让机器人走起来，先加速8秒，然后匀速运动一段，然后再减速。

robot = Robot()
robot.run(1,8)
robot.run(0,3)
robot.run(-1,8)
robot.show_locatons()

机器人位置随时间变化曲线

注意了哟，上面这个图是真实位置，而实际上机器人根本不知道自己的真实位置，也不知道自己的速度，它只能通过测距仪来测量其位置。但是测距仪的测量总是会有一定的误差，有测量就有误差，有误差就有估计。

再来一遍，有测量就有误差，有误差就有估计。

我们假设这个误差的大小和速度有关，速度越大测距误差越大。然后我们画出测量传感器返回的结果看看。

def __init__(self, state=0, speed=0):
  ...
  self.measure_locations = []
 
def run(self, acc=0, t=0):
  ...
  for i in range(t):
    ....
    # 测量的位置为真实位置加测量误差
    self.measure_locations.append(self.state + self.speed * np.random.randn())

真实位置与测量位置随时间变化曲线

Alpha beta filter

上面的图中我们看到，虽然黄色的线并没有偏离蓝色的线太多，但是每个时刻的测量误差还是太大，我们希望通过某种技术将这个波动缩小。

这就是信号处理计算里面的滤波技术，滤除噪声(测量误差)，保留信号。

That's why we call it "filter".

我们先假设这个机器人比较便宜，上面的测距仪质量非常差。机器人慢慢向前移动的过程中，测距仪返回的数字一直都是1m, 1.5m, 2.8m。 这时测距仪突然抽风了一下，返回了一个400m给机器人。机器人一下就懵逼了，“我前一秒还在2.8米的位置呢，怎么一下就跑这么远了？”。如果机器人还更聪明一点，它可能还会怀疑，“是不是我的测距仪错了？，那我到底现在在哪儿呢？”

“既然我前一秒的位置是在2.8米的地方，如果那时候我的位置没错的话，加上我还知道这段时间我运行的速度，那我应该可以算出我的位置。”机器人接着想。

“可是如果我的测距仪没坏呢？如果有一个运动飞快的人在一秒钟的时间内把我带到了这儿呢？或者我其实刚才丢掉了一些数据，其实我已经运动了半个小时了呢？”

“我到底该相信我的预测，还是相信我的测量？”，我们的机器人此时问出了最关键最关键的那个问题，“预测？还是测量？”

小孩才做选择，成年人全都要。既要预测，也要测量。不管信息对不对，不丢掉任何信息。

再来一遍哦，既要预测，也要测量，不丢掉任何信息。

先说说预测吧，要想预测，机器人得先知道自己的速度（状态变化的快慢）。一开始机器人不知道自己啥速度，就先认为自己速度是0吧，然后通过运动，他发现自己的位置在变化，于是它就可以根据这个位置的变化来计算自己的速度了。

在计算自己速度的过程中，机器人也很小心，因为它知道自己是一个便宜货（哭），他的传感器老是返回一些离谱的位置数据，这就导致它在计算自己位置的时候很容易出错，而位置一错速度的估计就错了，速度一错，位置的预测也就错了。这下好了，预测跟着测量一起错，还能相信谁？

于是机器人想到了那两个字，「惯性」。“和位置不可能说变就变一样，速度也不可能说变就变，一般情况下，每个时刻的速度应该和前一时刻的速度差不多才对。”，机器人接着想，“更棒的是，我预测出的位置和我测量的位置间的差，可能正反应了我速度的变化，我应该把这个也用上”。于是机器人决定自己测自己的速度。

class Robot:
  def __init__(self, state=0, speed=0, g=0.5, h=0.5):
    self.state = state
    self.speed = speed
    # 机器人的实际位置
    self.actual_locations = [state]
    self.measure_locations = []
    self.measure_speed = 0.0 # 初始速度不知道，就先0吧
    self.estimate_locations = [0.0]
    self.est_state = 0.0  # 初始位置不知道在哪，就先0吧
    self.g = 0.5
    self.h = 0.5
  
  # 这是一个只会做匀加速运动的机器人（匀速运动也是一种匀加速运动）
  def run(self, acc=0, t=0):
    if t <= 0: return
    for i in range(t):
      s1 = self.speed
      s2 = self.speed + acc
      self.state += (s1 + s2) / 2.0
      self.speed = s2

      # 测量位置等于实际位置加测量噪声
      measure_state = self.state + self.speed * np.random.randn()

      # 预测位置等于上一个时刻的预测位置 + 机器人认为的自己的速度
      pred_state = self.est_state + self.measure_speed * 1

      # residual 测量与预测的差距
      residual = measure_state - pred_state

      # 根据 residual 和上一个时刻机器人认为的自己的位置计算新的速度
      self.measure_speed = self.measure_speed + self.h * residual

      # 结合测量与预测估计当前位置
      self.est_state = pred_state + self.g * residual

注意上面代码中的g和h（通常在0到1之间），这两个系数用于表示机器人更相信自己的预测计算，还是传感器返回的测量数据。g和h越接近于0，表明机器人越相信自己的预测，也就是越相信自己的经验，而g和h越接近于1，则表示机器人越相信传感器传回的测量数据。

实验

我们先来个折中一点的机器人，g=0.5, h=0.5。它既不迷信自己的测量，也不偏向自己的预测能力。我们看到位置估计的曲线相比测量值平滑了不少，而且和实际值还是非常接近的。

尤其值得注意的是，图中有几个时刻测量位置已经远远偏离了实际位置，但是机器人还是通过alpha beta filter尽可能地过滤掉了这种测量噪声，让估计值比较靠近实际值。

g=0.5, h=0.5

下面请欣赏g=0.1, h=0.1版的蜜汁自信机器人，这家伙基本不太care传感器说啥，非常相信自己的经验。他热衷于预测，却对身边事物缺少观察，曲线平滑少有变化，真是个固执的家伙！

g=0.1, h=0.1

而下面这位g=0.9, h=0.9的机器人则非常相信它的传感器了，红线和黄线基本贴一起了。这家伙的毛病是几乎只观察不思考，要是哪天传感器坏了，它不知道会以为自己在哪里呢。

g=0.9, h=0.9

小结

本文中我们用alpha beta filter设计了一个机器人，它只要用一个距离传感器，就能比较准确的估计出自己当前的位置。

g和h的值是设计中的关键。如果传感器的质量非常好，或者更general的说，测量信号的信噪比更高，则可以考虑将g和h设置大一点。而如果估计的数字变化率相对比较低的，或者说状态的惯性比较大的话，则可以考虑将g和h设置小一点。具体的设置就case by case了。

而 alpha beta filter 真正的内核所在则是：

”估计 = 预测 + 测量“

或者用更bayesian的话说：

”posterior = prior + likelihood“

或者更general一点说：

Don't lose any information.

ps: 立个flag，下次准备写篇文章讲讲卡尔曼滤波，但是感觉内容太多可能讲不太清楚，试试看吧。

ps: 代码都在github上了，这里面我会陆续写一点和「估计」啊「滤波」啊有关的东西。另外，我还打算写一点和「预测」以及「近似」相关的文章。算是我的「估计、预测、近似」三部曲计划吧。

chenminhua/estimation​github.com

大家觉得有帮助的话记得点赞呀，不然没准过两个礼拜我就忘了这事儿了哈哈。