EKF 扩展卡尔曼滤波适用于全向底盘的修改方法

最近工作叫忙都没时间更新，今天周末记录一个使用 bfl 库完成 EKF 的小知识。
在EKF的预测中，使用的更新量默认为：距离+角度。因此默认是适用于差速底盘或者阿克曼底盘的，但是对于全向底盘，在不需要转向的前提下便可以完成倒退，就会使预测值发生问题。

先来看一下 bfl 库中差速地盘的代码：

  ColumnVector NonLinearAnalyticConditionalGaussianMobile::ExpectedValueGet() const
  {
    
    
    ColumnVector state = ConditionalArgumentGet(0);
    ColumnVector vel  = ConditionalArgumentGet(1);
    state(1) += cos(state(3)) * vel(1);
    state(2) += sin(state(3)) * vel(1);
    state(3) += vel(2); 
    return state + AdditiveNoiseMuGet();
  }

在 ExpectedValueGet 更新函数中，使用的是距离+角度的方式，此处的距离是一个非负值，因此在全向模型下，角度不发生改变的情况下倒退，就会使预测位置向前滑动的情况。因此为了适应全向模型，需要对改更新公式做出修改：

  ColumnVector NonLinearAnalyticConditionalGaussianMobile::ExpectedValueGet() const
  {
    
    
    ColumnVector state = ConditionalArgumentGet(0);
    ColumnVector vel  = ConditionalArgumentGet(1);
    state(1) = state(1) + cos(state(3)) * vel(1) - sin(state(3)) * vel(2);
    state(2) = state(2) + sin(state(3)) * vel(1) + cos(state(3)) * vel(2);
    state(3) += vel(3);
    return state + AdditiveNoiseMuGet();
  }

修改成如上所示的更新公司，更新值由距离+角度更新为deltaX+deltaY+角度，此处的差值是存在正负的，因此便可以解决角度不变发生后退的情况。

注意：在更改更新公式的同时，记得同时修改其雅科比矩阵 dfGet ！

---

题外话：提醒朋友们也是提醒自己，在与其他人对接口时，要把出参（引用）的值重新赋值，以防止对方随便设置默认值引发“血案”！