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


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

你可能感兴趣的:(ROS,定位算法,机器人,ROS)