将互补滤波器直接应用于矩阵

Question

到目前为止，我已经按照如下方式融合了运动传感器：

// 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)

但是如您所见，我将矩阵转换为方向，然后再将其转换回矩阵。

我想通过将互补滤波器直接应用于矩阵（而不将矩阵转换为方向）来提高性能。可能吗？

矩阵的大小为 9 (3x3)。

换句话说，我该怎么做？

MatrixC = 0.98 * MatrixA + 0.02 * MatrixB

Answer 1

我通过 :

解决了这个问题

1. 将矩阵的每个条目与标量相乘。

2. 根据条目坐标将矩阵的每个条目相加。