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.
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
1in 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.
