Ayuda con IMU 9-dof

Hola a todos, me gustaria saber si alguien me puede ayudar con un problema que tengo:

Estoy intentando configurar un sensor de movimiento inercial de 9 grados de libertad (gyro, accel y magn) usando como equipo un genuino 101 puesto que ya lleva el giroscopo y el acelerometro integrados y como magnetómetro estoy usando un HMC5883.

Por el momento he conseguido obtener por un lado las mediciones del giroscopo y acelerometro y representarlas en processing y lo mismo con el magnetómetro ambos de forma independiente.

El problema es que no se como combinar ahora las dos series de datos que tengo para conseguir una mejor estabilidad o precisión puesto que solo con el giroscopo y el acelerometro el sistema tiene un error bastante significativo.

investigando he llegado a la conclusion que deberia de usar un filtro de Kalman o similar para combinar los datos pero no tengo ni idea de como conseguirlo

Muchas gracias por la ayuda, Un saludo