I am trying to achieve this and also this, but in Unity3D and using an IMU ICM-20948 9-DOF sensor:
So far I was able to use the Madgwick's algorithm and rotate a cube by tilting the sensor in the air. I am not sure now how to make it move like a mouse pointer. What I experience is - when I move the sensor up and down (kind of tilting it up as if it is a remote control), seems working. But when I tilt it left and right, it doesn't work. Also the values jump from plus to minus rapidly, which makes the object appear on the opposite side of the screen - this is for x and y. The only way it works, is if I move it very lightly, but then it just stays within a very small range. I want to be able to move it anywhere on the screen.
Here is my code:
MadgwickAHRS madgwickAHRS = new MadgwickAHRS(1f / 256f, 0.1f);
madgwickAHRS.Update(gx, gy, gz, ax, ay, az, mx, my, mz); // called in a separate thread loop
float[] angles = ToEuler(madgwickAHRS.Quaternion);
float horzValue = angles[0] - horzZero;
float vertValue = angles[1] - vertZero;
horzZero = angles[0]; // yaw
vertZero = angles[1]; // roll
void FixedUpdate() {
transform.position = new Vector3(horzValue, vertValue, 0);
}
...
float[] ToEuler(float[] q)
{
float yaw = (float)Math.Atan2(2.0 * (q[1] * q[2] + q[3] * q[0]), q[3] * q[3] - q[0] * q[0] - q[1] * q[1] + q[2] * q[2]);
float pitch = (float)Math.Asin(-2.0 * (q[0] * q[2] - q[3] * q[1]));
float roll = (float)Math.Atan2(2.0 * (q[0] * q[1] + q[3] * q[2]), q[3] * q[3] + q[0] * q[0] - q[1] * q[1] - q[2] * q[2]);
return new float[2] { yaw, roll };
}
Tried to use this function as well for each of the values in angles, but it makes things worse.
private float CalcDist(float angle, float initAngle)
{
angle = (float)((angle - initAngle) * (180 / Math.PI));
angle = angle < 0 ? angle + 360 : angle;
angle = angle > 180 ? angle - 360 : angle;
float dist = (float)(Math.Round(-offsetDistance * Math.Tan(angle * (Math.PI / 180))));
return dist;
}
Also tried to use corrected quaternion (this works well for the rotation by the way):
Quaternion objectOrientation = new Quaternion(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
var qEualr = objectOrientation.eulerAngles;
Quaternion correctQuaternion = Quaternion.Euler(qEualr.z, -qEualr.y, qEualr.x);
Could someone here help me?