I need to use the gyroscope sensor to get the orientation

Question

I'm working on Xamarin and I need to use only the gyroscope sensor to get the orientation of the phone on a table when I turn it in the 0-360 degree range. I already probe using the fusion of this sensor with the accelerometer but I can not find an understandable code in java or C #. I also try to calculate the angles so with the gyroscope reading but when I turn it fast or I delay in rotating the phone the sensor tends to give me a bad reading, the same increases the degrees that decrease them. I have read in the bibliography that a filter should be applied to the calculation but I have not found code in java or C # to test. Here I put the code that I am using only for gyroscope. Please excuse my English.

if (e.Sensor.Type == SensorType.Gyroscope)
                    {
                        mAzimuth = this.gyroFunction(e);
                    }   

public float gyroFunction(SensorEvent e)
             {
                        float[] deltaVector = new float[4];
                    if (timestamp != 0)
                    {
                        float dT = (e.Timestamp - timestamp) * NS2S;
                        Array.Copy(e.Values.ToArray(), 0, gyro, 0, 3);
                        getRotationVectorFromGyro(gyro, deltaVector, dT / 2.0f);
                    }

                    // measurement done, save current time for next interval
                    timestamp = e.Timestamp;

                    // convert rotation vector into rotation matrix
                    float[] deltaMatrix = new float[9];
                    SensorManager.GetRotationMatrixFromVector(deltaMatrix, deltaVector);

                    // apply the new rotation interval on the gyroscope based rotation matrix
                    gyroMatrix = matrixMultiplication(gyroMatrix, deltaMatrix);

                    // get the gyroscope based orientation from the rotation matrix
                    var angles = SensorManager.GetOrientation(gyroMatrix, gyroOrientation);
                    mAzimuth = (float)(this.RadianToDegree(angles[0]) + 360) % 360;
                    return mAzimuth;
             }

              private void getRotationVectorFromGyro(float[] gyroValues,
                                               float[] deltaRotationVector,
                                               float timeFactor)
        {
            float[] normValues = new float[3];

            // Calculate the angular speed of the sample
            float omegaMagnitude =
                (float)Math.Sqrt(gyroValues[0] * gyroValues[0] +
                                 gyroValues[1] * gyroValues[1] +
                                 gyroValues[2] * gyroValues[2]);

            // Normalize the rotation vector if it's big enough to get the axis
            if (omegaMagnitude > EPSILON)
            {
                normValues[0] = gyroValues[0] / omegaMagnitude;
                normValues[1] = gyroValues[1] / omegaMagnitude;
                normValues[2] = gyroValues[2] / omegaMagnitude;
            }

            // Integrate around this axis with the angular speed by the timestep
            // in order to get a delta rotation from this sample over the timestep
            // We will convert this axis-angle representation of the delta rotation
            // into a quaternion before turning it into the rotation matrix.
            float thetaOverTwo = omegaMagnitude * timeFactor;
            float sinThetaOverTwo = (float)Math.Sin(thetaOverTwo);
            float cosThetaOverTwo = (float)Math.Cos(thetaOverTwo);
            deltaRotationVector[0] = sinThetaOverTwo * normValues[0];
            deltaRotationVector[1] = sinThetaOverTwo * normValues[1];
            deltaRotationVector[2] = sinThetaOverTwo * normValues[2];
            deltaRotationVector[3] = cosThetaOverTwo;
        }

         private float[] matrixMultiplication(float[] A, float[] B)
        {
            float[] result = new float[9];

            result[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6];
            result[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7];
            result[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8];

            result[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6];
            result[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7];
            result[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8];

            result[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6];
            result[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7];
            result[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8];

            return result;
        }

        private double RadianToDegree(
            double angle)
        {
            return angle * (180.0 / Math.PI);
        }

Answer 1

I managed to get a working code version for obtaining position with the Accelerometer. I am using the Xamarin.Essentials Nuget packet, with this code:

1. First, obtained from Microsoft documentation and made little modifications, I turn on the accelerometer:

   public void ToggleAcceleromter()
   {
       try
       {
           if (Accelerometer.IsMonitoring)
           {
               Accelerometer.Stop();
               Accelerometer.ReadingChanged -= Accelerometer_ReadingChanged;
               UserDialogs.Instance.Toast("Gyroscope mode disabled, tap again to enable.");
           }
           else
           {
               Accelerometer.Start(SensorSpeed.Ui);
               Accelerometer.ReadingChanged += Accelerometer_ReadingChanged;
               UserDialogs.Instance.Toast("Gyroscope mode enabled, tap again to disable.");
           }
       }
       catch (FeatureNotSupportedException fnsEx)
       {
           UserDialogs.Instance.Toast("Gyroscope mode not supported in this device.");
       }
       catch (Exception ex)
       {
           UserDialogs.Instance.Toast("Gyroscope unkown error, please report tu bugs@xxxx.com");
       }
   }
   

Then, in the handler, I used this code:

    void Accelerometer_ReadingChanged(AccelerometerChangedEventArgs e)
    {
        var data = e.Reading;

        var roll = Math.Atan(data.Acceleration.Y / Math.Sqrt(Math.Pow(data.Acceleration.X, 2.0) + Math.Pow(data.Acceleration.Z, 2.0)));
        var pitch = Math.Atan(data.Acceleration.X / Math.Sqrt(Math.Pow(data.Acceleration.Y, 2.0) + Math.Pow(data.Acceleration.Z, 2.0)));
        var yaw = Math.Atan(Math.Sqrt(Math.Pow(data.Acceleration.X, 2.0) + Math.Pow(data.Acceleration.Z, 2.0)) / data.Acceleration.Z);

        Debug.WriteLine("roll: " + roll.ToString() + ", pitch: " + pitch.ToString() + "yaw: " + yaw.ToString());
    }

In those three variables, we can see a steady number representing the position of the device in three different axis. In my case, I wanted to get a number from 0 to 100 for the movement in the frontal axis (pitch). And I did it with this code, which shows the value in an entry and moves a slider:

    double unsignedYaw = 0;
        if (pitch < 0) {
            unsignedYaw = pitch * -1;
        } else {
            unsignedYaw = pitch;
        }

        double scaleMiddle = 50;
        var scaledYaw = unsignedYaw * scaleMiddle / 0.5;

        double finalValue;
        if (pitch > 0) {
            finalValue = scaleMiddle + scaledYaw; 
        } else {
            finalValue = scaleMiddle - scaledYaw;
        }

        if (finalValue < 1) {
            moveValue.Text = "1";
            sliderSteps.Value = 1;
        } else if (finalValue > 1 && finalValue <= 100) {
            moveValue.Text = finalValue.ToString();
            sliderSteps.Value = finalValue;
        }