Structuring EKF to estimate pose and velocity with odometry inputs

Question

I have a differential drive robot for which I'm building an EKF localization system. I would like to be able to estimate the state of the robot $\left[ x, y, \theta, v, \omega \right]$ where $x, y, \theta$ represent the pose of the robot in global coordinates, and $v, \omega$ are the translational and rotational velocities. Every mobile robot Kalman filter example I've seen uses these velocities as inputs to prediction phase, and does not provide a filtered estimate of them.

Q: What is the best way to structure a filter so that I can estimate my velocities and use my measured odometry, gyroscope, and possibly accelerometers (adding $\dot{v}$ and $\dot{\omega}$ to my state) as inputs?

My intuition tells me to use a prediction step that is pure feedforward (i.e. just integrates the predicted velocities into the positions), and then have separate updates for odometry, gyro, and accelerometer, but I have never seen anyone do this before. Does this seem like a reasonable approach?

Answer 1

Great question! Your intuition is correct.

I would do this:

1. Put the state $x$, and expected velocities at the current time $t$, $v $ as the vector to estimate.
2. Use estimated velocities to predict next state $$\hat{x}(t+1)=f(x(t),v(t))$$
3. Use odometry measurements from $t$ to $t+1$ to form an estimate of the state. The difference between this and $x$ is your innovation, $y_o$. Fuse with the predicted state. $$\hat{x}'(t+1)=UpdateStep(\hat{x}(t+1),y_o(t+1)$$
4. Use accellerometers to form a second estimate of state, which is also fused. You now have an estimate of the state at $t+1$ using all available information. $$\hat{x}''(t+1)=UpdateStep(\hat{x}'(t+1),y_a(t+1)$$
5. Now, the velocities, assuming the filter is running "fast" and you can rely on a constant curvature path are whatever $v$ and $\omega$ explain the difference in the robot states. Use the difference to provide an estimate of the velocities, which is used to update the velocities in the state of the robot. $$\hat{v}(t+1)=UpdateStep(\hat{v}(t+1),GetVelocityEstimate(\hat{x}(t),\hat{x}(t+1))$$

The design of a filter is very much up to the engineer, and in the end you may want to try several variants to see what works best.

Some other variants

• Swap odometry, velocities, and accells to produce the prediction of state. You'll have to see what works better
• Use a single integration of accellerometers to produce a velocity update before doing your forward prediction. In effect, you are "daisy chaining" EKFS, one to estimate your velocity, then using that velocity to predict your state, which is updated by other means. This will likely be much simpler and less likely to break.

Good luck!

Answer 2

The OpenPilot EKF implementation as described at the OpenPilot Firmware Development INSGPS Algorithm page does have the velocity vector as part of its state. The linked paper GPS-aided INS Solution for OpenPilot by Dale E. Schinstock, Kansas State University describes in quite a bit of detail how they've implemented it.