0

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.

Awoooooooooooooooooooo
  • 157
  • 9
Ahmed Sobhy
  • 1

1 Answers1

0

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.

imuengine.io
  • 51
  • 3
  • Thank you for your reply. The problem with this method is that I find the orientation of the robot drift much faster from reality. I think that this might be due to a high variance in the steering wheel angle measurement. This is why I think if I use the measurement variance of the steering wheel to tune the filter then this would give better results. When using a covariance matrix of the states to tune for that error it is not possible to isolate the effect of the error in the wheel angle. – Ahmed Sobhy Jun 04 '23 at 19:41
  • No problem. You mentioned "the orientation of the robot drift much faster from reality". Can you explain further? Is the filter too conservative (i.e. estimated variance > true error) or too optimistic (i.e. estimated variance < true error)? And how do you know the problem is not with the IMU part of the filter? – imuengine.io Jun 05 '23 at 02:54