Search code examples
javaandroidkotlinmatrixsensors

Apply a complementary filter directly to matrices

Until now I have fused the motion sensors as follows :

// Get Static Orientation (Accelerometer + Magnetometer)
SensorManager.getOrientation(staticRotationMatrix, staticOrientation)

// Get Dynamic Orientation (Gyroscope)
SensorManager.getOrientation(dynamicRotationMatrix, dynamicOrientation)

// Fuse Orientations (Complementary Filter)
fusedOrientation[0] = coefficient * dynamicOrientation[0] + (1.0F - coefficient) * staticOrientation[0]
fusedOrientation[1] = coefficient * dynamicOrientation[1] + (1.0F - coefficient) * staticOrientation[1]
fusedOrientation[2] = coefficient * dynamicOrientation[2] + (1.0F - coefficient) * staticOrientation[2]

// Remove Gyroscope Drift
dynamicRotationMatrix = getRotationMatrixFromOrientation(fusedOrientation)

But as you can see I convert the matrices to orientations and then convert it back to matrices.

I would like to improve the performance by applying the complementary filter directly to the matrices (without converting the matrices to orientations). Is it possible?

The matrices have a size of 9 (3x3).

In other words, how can I do this?

MatrixC = 0.98 * MatrixA + 0.02 * MatrixB
Solution

  • I solved the problem by :

    1. Multiplying each entry of the matrix with the scalar.

    2. Adding up each entry of the matrices according to their entry coordinates.