Multi-Rate Sensor Fusion using EKF

Question

Context: I have an IMU(a/g/m) + Wheel Odometry measurement data that I'm trying to fuse in order to localize a 2D (ackermann drive) robot.

The state vector X = [x y yaw]. I'm using the odometry data to propagate the state through time (no control input). The update step includes the measurement vector Z = [x_odo y_odo yaw_imu].

I have two questions:

1.Does it make sense to use the odometry data(v_linear, omega) in both the prediction as well as update steps?

2.How do I account for the frequency difference between the odometry data(10Hz) and the imu data(40Hz)? Do I run the filter at the lower frequency, do I dynamically change the matrix sizes or is there any other way?

Thanks!

Answer 1

Part 1. Use one or the other. Often odometery is used instead of kinematics or dynamics for prediction, at least in my work.

Part 2. This is handled by the construction of the measurement equation jacobian.

Every time a measurement comes in, construct a Jacobian for the whole state. You'll notice that some of the state elements are independent of the current measurement (vel sensor cannot measure position, e.g.). So, no need to resize the matrix, just run the ekf with some zeros in the jacobian.

Specifically, for every incoming measurement

1. Predict state from last update to the current
2. Construct jacobian for current measurement type / equation
3. Update state

Answer 2

I've performed 2D localization with just odometry and a gyroscope before, and to be honest, depending on (i) how good your encoders are; (ii) what type of environment you're in (is there a chance your wheels will slip a lot); (iii) how good your IMU is, there's a good chance that you don't lose much by just using odometry for translation, and only calculating heading using the gyroscope. In other words, just update the position when you get an odometry measurement, and just update the heading when you get a gyroscope measurement (i.e., two prediction steps, no correction step). Practically speaking, you won't get much better results using a correction step when you aren't measuring the environment.

That being said, I noticed that you didn't mention anything about using the magnetometer of your IMU to estimate the heading of the robot. If you are in a somewhat magnetically clean environment (i.e., you can somewhat reliably measure the Earth's magnetic field), then this measurement would be extremely useful. The reason why is because it is an exteroceptive sensor (i.e., it's measuring the environment) and your other sensors (wheel encoders, gyroscope) are interoceptive sensors (i.e., they measure the robot internally). With only interoceptive sensors measuring your heading, it is subject to dead reckoning and its uncertainty will increase without bounds. And as you know, if your heading becomes increasingly uncertain, so does your position. With an exteroceptive sensor measurement of your heading, its uncertainty becomes bounded.

Here's one way to set up your filter:

• Each time you receive an odometry measurement (10 Hz), perform the prediction step to propagate your state forward.

• Each time you receive an IMU measurement (40 Hz), perform a (different) prediction step to propagate your heading estimate forward by integrating the gyroscope, then perform a correction step to correct your heading using the magnetometer.

To use the magnetometer in this way, you need to know the direction of the Earth's magnetic field in your local area (you can look this up online). Let's call this $m\in[-\pi,\pi)$. Now you get a measurement of the magnetic field $z_k\in[-\pi,\pi)$ from your magnetometer.

Your measurement model is:

$$ \hat{z}_k = m - \hat{\theta}_k $$

where $\hat{\theta}_k$ is your current heading estimate. Note: you need to account for wrapping of the angles here! One way to do this is:

$$ \hat{z}_k = \text{atan2}(\sin(m - \hat{\theta}_k),\cos(m - \hat{\theta}_k)) $$

but there are other ways as well. Then just proceed with the normal KF correction step.