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()


  • 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.

  • $\begingroup$ To find the Jacobian you'll have to start with the equations of motion. Do you have these? $\endgroup$
    – holmeski
    Aug 10, 2020 at 11:48
  • $\begingroup$ 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. $\endgroup$
    – edwinem
    Aug 10, 2020 at 12:32
  • $\begingroup$ Hello @holmeski I have the equations of the model, but not accounting for bias, so that is where I am stuck $\endgroup$ Aug 10, 2020 at 15:02
  • $\begingroup$ Hi @edwinem thanks a ton! I'll check it out $\endgroup$ Aug 10, 2020 at 15:02
  • 1
    $\begingroup$ 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. $\endgroup$
    – edwinem
    Aug 13, 2020 at 4:59


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Browse other questions tagged or ask your own question.