how to handle vehicle speed and wheel angle covariance in a kalman filter

Question

When fusing IMU, vehicle speed, and wheel angle, I am having trouble converting vehicle speed and wheel angle covariance to system state covariance.

I am using a Kalman filter to fuse the readings from an IMU and a vehicle.

IMUs provide linear acceleration and angular velocity. The vehicle's CAN is providing speed and wheel angle.

I am using a simplified bicycle model.

Assuming constant acceleration, the vehicle's state is as follows:

x x_dot x_2dot y y_dot y_2dot theta theta_dot

Using the bicycle model, I can calculate the following states of the vehicle given speed and wheel angle: x, x_dot, y, y_dot, theta, theta_dot, but I am not sure how to handle the covariance of the speed and wheel angle.

I thought about this problem in two different ways. First, convert wheel angle and speed to the states above, then create an observation matrix H that maps the measurement to the prediction one to one, then transform the 2x2 speed and wheel angle covariance matrix into a 6x6 matrix for the calculated states x, x_dot, y, y_dot, theta, theta_dot.

The second way is to feed the Kalman filter the wheel angle and speed but modify the observation matrix so that it can transform the predicted state by the Kalman filter into wheel and speed, but I could not find a way to do so.

Answer 1

A multi-step approach comes to mind:

1. Start with the first approach (convert wheel angle and speed into the states and fuse them using the standard Kalman filter measurement equations. The H matrix will be 1's and 0's. Don't worry about getting the measurement covariance exactly right. Just assume a reasonable fixed value for each state (e.g. 1 m^2 for position and 0.25 (m/s)^2 for velocity).

2. See how it works. You may find it is good enough. Or, perhaps it will show you need more sensors. After all, both the IMU and wheel odometry will drift with time/distance. If everything looks good and you want a more exact measurement covariance matrix, then proceed to step 3.

3. Take the equations you are using for mapping the measurements into states. These are likely non-linear. Linearize them and then use standard linear methods for propagating the measurement error into state errors.