Modification method of EKF extended Kalman filter suitable for omnidirectional chassis

I've been busy with work recently and haven't had time to update. Today, I will record a little knowledge using bflthe library this weekend. In EKF prediction, the update amount used defaults to: distance + angle . Therefore, the default is suitable for differential chassis or Ackerman chassis, but for omnidirectional chassis, reverse can be completed without steering, which will cause problems with the predicted value.EKF

Let’s first take a look at bflthe code for the differential chassis in the library:

  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();
  }

In ExpectedValueGetthe update function, the method of distance + angle is used. The distance here is a non-negative value. Therefore, under the omnidirectional model, going backward without changing the angle will cause the predicted position to slide forward. . Therefore, in order to adapt to the omnidirectional model, the update formula needs to be modified:

  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();
  }

Modify it to the update company as shown above. The update value is updated from distance + angle to deltaX + deltaY + angle . The difference here is positive and negative, so it can solve the problem of retreat when the angle does not change.

Note: When changing the update formula, remember to modify its Jacobian matrix at the same time dfGet!

---

Off-topic : I remind my friends to also remind myself that when interacting with other people, the values ​​of the parameters (references) must be reassigned to prevent the other party from randomly setting the default value and causing a "murderous crime"!