# IMU Vision Fusion using EKF

I am trying to track an object indoors using an IMU (only accel and gyroscope) and a visual marker. This is similar to IMU+GPS fusion, where GPS is effectively replaced by the position that my vision based system gives based on the marker. I am writing an error EFK to fuse the two readings. I am including the accel and gyro biases as part of my state estimation as well, to reduce the drift caused by IMU in between measurements. For the same, I have a couple of queries.

1. What are the jacobians for the state uncertainty propogation in the prediction step. Here are my notations:

P = F * P * F.t() + G * L * G.t()

where

• P is my state covariance matrix (15x15) - 3 for position, 3 for velocity, 4 for orientation (quaternion) and 6 for accel and gyro biases
• F is the jacobian of prediction model. It should be of size 15x15. For the first 9 rows, it is straightforward, however, I am not sure about the last 6 rows (that correspond to the biases)
• L is the variance and bias variances of the accel and the gyroscope (12x12) (a simple diagonal matrix)
• Finally G is the jacobian associated with the noise and biases. I am not sure what this should be as well. It should be a (15x12) matrix. The first 3 rows are zeros, but dont know how to fill the rest of the matrix. Is this also a simple diagonal matrix (from row 4) with values 1 in the diagonal? I am confused primarily about this matrix.

Any help on deriving these jacobian matrices would be very helpful!

Finally, will my method work? I.e. I follow a pipeline similar to IMU+GPS fusion, however I replace the GPS component with my visual marker position estimate.

• To find the Jacobian you'll have to start with the equations of motion. Do you have these? – holmeski Aug 10 '20 at 11:48
• The best paper about Error State EKFs is the seminal paper by Joan Sola. Lucky for you he derives all of the jacobians and other equations for an IMU+GPS based system. – edwinem Aug 10 '20 at 12:32
• Hello @holmeski I have the equations of the model, but not accounting for bias, so that is where I am stuck – OlorinIstari Aug 10 '20 at 15:02
• Hi @edwinem thanks a ton! I'll check it out – OlorinIstari Aug 10 '20 at 15:02
• IMU translation is always terrible due to it providing acceleration information and the double integration you have to do. 2 seconds is large enough that I would expect it to drift a significant amount. You really do need to have the visual update happen also at a pretty high rate. At least 1-2 times a second. I would also double check that you are doing everything correctly by looking at the rotation. If the rotation drifts a lot then you are doing something wrong. – edwinem Aug 13 '20 at 4:59