# How to do IMU and camera "sensor fusion" tracking?

I have some 50ms latency cameras on hand and an gyro+accelerometer+magnetometer 800Hz IMU. I would like to know how exactly I should do a sensor fusion of such an IMU and camera to fix the positional data from the IMU positional drift. I'm not able to find much resources online.

Reason I don't want to go with just camera is the latency of 50ms with it. ​ The optical markers for the camera can be LEDs, ORB-SLAM data or AruCo markers which I currently use which add another few ms latency to the camera tracking. ​ Maybe there is even an existing library or documented implementation I can use? ​ Thank you.

Begin by finding a way to convert SLAM or ArUco data into absolute position in some coordinate system. This is most complex part and I have no idea how to to this, never have worked with either.

Then apply Kalman filter to accelerometer and position data, exactly same way as it is done for gyro and accelerometer (there are plenty of examples for that on the web, including hundreds of videos on Youtube).

The idea is to integrate accelerometer over time twice to get speed first and position second. The result would be fast position value with severe drift. Then you apply noisy and slow absolute position from camera to correct that drift.

This is the same as integrating gyro data to get fast angle with drift and then correcting that drift with accelerometer/magnetometer inputs.

• Are there existing libraries that will take position and rotation data from both (9DOF IMU output and camera position tracking output) and return new position data from the algorithm? Not trying to be lazy but not wanting to reinvent the wheel either. Commented Sep 3, 2018 at 9:14
• Not that I know of. All code I've seen before worked for orientation, not position. From your comment on the other answer I see you already have fusion of IMU data. So you should have filtered acceleration and corrected attitude. Integrate acceleration to get speed. Use attitude angles to transform speed vectors into same coordinate system as your camera output. Apply Kalman filter same way it has been done for IMU, only use speed instead of gyro and position instead of angle. Commented Sep 3, 2018 at 10:21

IMO:

First of all you should implement 9 DOF fusion algorithm with some very high speed MCU, to obtain attitude at high sample rate. These algorithm was implemented by Magdwick, think is the most used for those MEMS sensors, but there others DCM, Kalman,...some are better but have also more computational burden. Then you should extrapolate the acceleration for every axis XYZ and integrate to get velocity, then again to get the position.

I had previously made integration of acceloremeter data and compared the results with high speed camera. The setup consisted of a running bench, high speed cameras and runner with 3 axis accelerometer with datalogger on gait, marker, cameras anlysis software that outputs 3D coordinates. Both logs were compared in Matlab.

The results were good, but not satisfying, because of contrifugal acceleartion during twisting,...etc. The sensor was 3 axis acceleorometer only. So the conclusion was, that you would need an attitude estimator aka IMU sensor and only then you can get filtered components.

issue 1: Integration of acceleration leads to infinite velocity due to drift. My first preasumption was that mean velocity is zero since the subject didn't fly away. So I used filtfilt() in Matlab to subtract the mean value (moving average) from computed velocity, leaving only the dynamic change.

isuue 2: Integration of velocity also gives a runaway of position. 2nd assumption: the mean position is zero. Again subtract the moving average from integrator.

issue3: The camera and dataloger were not synced, the XTALs did not produce the exact frequency and sampling rates were slightly different. The signal had to be resampled in Matlab (manually).

Conclusion: Magdwick algo uses quaternions, which I don't know. Perhaps you can find a simple way to decompose the 3D attiude information and get only the true accel. over all axes. Another option is to use trigonometric functions, some MCUs like cortex M4 have some basic DSP instructions, for example CORDIC (Coordiate Rotation DIgital Computer).

• I'm sorry for not being clear on this. The IMU I use already does the combination o data from accelerometer, gyroscope and magnetometer which are all included in the same IC. I just need to use the data (x,y,z position, euler rotation vector) from the camera tracker which is accurate but updates slower and with more latency to correct the drift from the fast 500Hz+ IMU. I was hoping there would be a library meant for this or having a function/method for doing this. Commented Sep 3, 2018 at 9:17
• @MarkLegault Then you should use the Kalman filter. You will find many accademic material if you search for GPS/IMU fusion. Your app isn't mauch different as it uses cameras instead of GPS. Commented Sep 3, 2018 at 10:33