将向量从手机坐标系转化到地球坐标系的过程详解和getRotationMatrix()详解

我们从手机传感器上得到的数据都是相对于手机坐标的，在行走过程中手机在不断晃动，它的坐标系也在不断变换，不便于我们进行实际运算，因此我们需要找一个相对稳定的坐标系，而地球坐标系就符合我们的需求。

（手机坐标系，手机头的指向为Y轴，手机屏幕的指向为Z轴，手机左右的指向为X轴）
首先应该知道，通过转化矩阵，可以将一个点从原来的坐标系转换到另一个坐标系中，如下：

$$\begin{bmatrix} A1 & A2 & A3 \\ B1 & B2 & B3 \\ C1 & C2 & C3 \end{bmatrix} *\begin{bmatrix} x \\ y \\ z \end{bmatrix}=\begin{bmatrix} x' \\ y'\\ z' \end{bmatrix}$$
假设（x,y,z）是在手机坐标上的点，通过转换矩阵所求得的（x’,y’,z’）是该点在地球坐标上的表示。

那么问题来了，这个转换矩阵该怎么求？
首先给出getRotationMatrix()的源码（该代码就是求转换矩阵的）
public static boolean getRotationMatrix(float[] R, float[] I,
float[] gravity, float[] geomagnetic) {
// TODO: move this to native code for efficiency
float Ax = gravity[0];
float Ay = gravity[1];
float Az = gravity[2];
final float Ex = geomagnetic[0];
final float Ey = geomagnetic[1];
final float Ez = geomagnetic[2];
float Hx = Ey*Az - Ez*Ay;
float Hy = Ez*Ax - Ex*Az;
float Hz = Ex*Ay - Ey*Ax;
final float normH = (float)Math.sqrt(Hx*Hx + Hy*Hy + Hz*Hz);
if (normH < 0.1f) {
// device is close to free fall (or in space?), or close to
// magnetic north pole. Typical values are > 100.
return false;
}
final float invH = 1.0f / normH;
Hx *= invH;
Hy *= invH;
Hz *= invH;
final float invA = 1.0f / (float)Math.sqrt(Ax*Ax + Ay*Ay + Az*Az);
Ax *= invA;
Ay *= invA;
Az *= invA;
final float Mx = Ay*Hz - Az*Hy;
final float My = Az*Hx - Ax*Hz;
final float Mz = Ax*Hy - Ay*Hx;
if (R != null) {
if (R.length == 9) {
R[0] = Hx; R[1] = Hy; R[2] = Hz;
R[3] = Mx; R[4] = My; R[5] = Mz;
R[6] = Ax; R[7] = Ay; R[8] = Az;
} else if (R.length == 16) {
R[0] = Hx; R[1] = Hy; R[2] = Hz; R[3] = 0;
R[4] = Mx; R[5] = My; R[6] = Mz; R[7] = 0;
R[8] = Ax; R[9] = Ay; R[10] = Az; R[11] = 0;
R[12] = 0; R[13] = 0; R[14] = 0; R[15] = 1;
}
}
if (I != null) {
// compute the inclination matrix by projecting the geomagnetic
// vector onto the Z (gravity) and X (horizontal component
// of geomagnetic vector) axes.
final float invE = 1.0f / (float)Math.sqrt(Ex*Ex + Ey*Ey + Ez*Ez);
final float c = (Ex*Mx + Ey*My + Ez*Mz) * invE;
final float s = (Ex*Ax + Ey*Ay + Ez*Az) * invE;
if (I.length == 9) {
I[0] = 1; I[1] = 0; I[2] = 0;
I[3] = 0; I[4] = c; I[5] = s;
I[6] = 0; I[7] =-s; I[8] = c;
} else if (I.length == 16) {
I[0] = 1; I[1] = 0; I[2] = 0;
I[4] = 0; I[5] = c; I[6] = s;
I[8] = 0; I[9] =-s; I[10]= c;
I[3] = I[7] = I[11] = I[12] = I[13] = I[14] = 0;
I[15] = 1;
}
}
return true;
}
gravity[]表示重力加速度g的向量投影在手机坐标上的x、y、z轴三个分量,geomagnetic[]表示地磁向量投影在手机坐标上的三个分，首先对它俩做一个叉乘，得到的向量H垂直于它俩所构成的平面，也就是指向东西方向（自行画图脑补），当然此时的向量H也是投影在手机坐标系上，然后将向量H和重力加速度向量A再做一次叉乘，得到的向量M指向南北方向（有人问为什么还要再求一次南北方向的向量，因为geomagnetic[]所得到的向量虽然由北指向南的，但它并不是正切于地表，而是南端略向地表倾斜），将所求得的两个向量H、M和重力向量A标准化后所组合成的矩阵就是转换矩阵：