# Multi-Rate Sensor Fusion using EKF

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!

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
• Thanks for the answer! In this case, the H matrix (state->meas space) will change according to the data available at the current time. But, won't this lead to yaw_imu being corrected more often than x_odo and y_odo, leading to an 'imbalance' in accuracy of the two? Will that be a problem? – AKag Feb 12 '16 at 4:31
• Not imbalance per se. If yaw is updated by two sources, it will just be a bit more accurate. That makes intuitive sense, I hope. – Josh Vander Hook Feb 12 '16 at 5:16

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.

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.

$$\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))$$