Tracking 3D object in unity with sensortag

See more:

C#

Android

Unity3D

oculus

Hi, I post some questions on a Project I started to develop few weeks ago. Maybe someone who read could make some suggestions
The projec is about Rotating 3d Objects in VR, made with unity. What I want to do is to use a SensorTag CC2650 (an IMU sensor with gyro, accelerometer and magnetometer) to rotate 3d Objects in VR with Oculus Gear. I would like to have results like this: IMU and AHRS.

I developed an Android Plugin for Unity to connect the BLE SensorTag and receive raw data. And it's all ok. The plugin produces a 9 components array for Gyroscope, Accelerometer, and Magnetometer. Now I have to integrate a Filter because of a gyroscope or accelerometer data are unusable alone due to the drifts.

So I tried to use the MadgwickAHRS algorithm found on the X-IMU project page in Unity but my results are inconsistent.

Maybe someone can help me on how to move and tell me if what I want to do is realizable.

What I have tried:

The SensorTag sample rate is set to 100ms. I'm using MadgwickAHRS class and I paste some unity example code just to see if what I'm trying to do is wrong.

public class Rotation : MonoBehaviour {
public AHRS.MadgwickAHRS AHRS;
float rotationSpeed = 2.5f;


void Start () {
    AHRS = new AHRS.MadgwickAHRS(0.1f);
}

// Update is called once per frame
void Update()
{
   AHRS.Update(deg2rad(STController.gyr[0]), deg2rad(STController.gyr[1]), deg2rad(STController.gyr[2]), STController.acc[0], STController.acc[1], STController.acc[2], STController.mag[0], STController.mag[1], STController.mag[2]);
        Quaternion targetRotation = new Quaternion(AHRS.Quaternion[0], AHRS.Quaternion[1], AHRS.Quaternion[2], HRS.Quaternion[3]);
     transform.rotation = Quaternion.Slerp(transform.rotation, targetRotation,rotationSpeed*Time.deltaTime);
}


static float deg2rad(float degrees)
{
    return (float)(Math.PI / 180) * degrees;
}

Any suggestions would be apreciated.

Thanks, Federico.