加速度和方向融合

世界坐标系为右手坐标系，z轴向下。加速度计坐标系也为右手坐标系。
加速度数度为g=[gx,gy,gz],heading为加速度计方向角
heading_v=[cos(heading), sin(heading), 0.0]

A为将世界坐标向量转为加速度计坐标系向量的旋转矩阵
   [ a00  a01  a02
A=   a10  a11  a12
     a20  a21  a22 ]

g为加速度向量，已经归一化，则由于g的方向在世界坐标下的向量为垂直向下（假设加速度计为静止状态）

A*[0.0,0.0,-1.0].T=g，所以
a02 = -g[0]
a12 = -g[1]
a22 = -g[2]

加速度计中的x轴，在全局坐标系中x-y平面的投影与heading_v同向，所以

[ 1 0 0            [ 1
  0 1 0   *  A.T *   0    =  n * heading_v
  0 0 0 ]            0 ]

由旋转矩阵的性质，得到
heading_v.T * heading_v+a02*a02=1.0
[a00,a01]=heading_v/norm(heading_v)*sqrt(1.0-a02*a02) 

又由向量外积性质得到
a20 = a01*a12-a11*a02
a21 = a02*a10-a00*a12
a00 = a11*a22-a12*a21
a01 = a12*a20-a10*a22
整理得到方程组，四个未知数四个方程
a20 + a02*a11 = a01*a12
a21 - a02*a10 = -a00*a12
a22*a11 - a12*a21 = a00
a12*a20 - a22*a10 = a01

[ 0    a02    1     0        [a10        [a01*a12
  -a02 0      0     1     *   a11    =    -a00*a12
  0    a22    0     -a12      a20         a00
  -a22 0      a12   0   ]     a21]        a01      ]

由此可以解得a10,a11,a20,a21

至此A矩阵中所有参数全部解出。

例如heading为30°对应得弧度，g=[0.2,0.9,9.8]可以得到

A= [[ 0.86620422  0.50010324 -0.02031845]
      [ 0.49650051 -0.86368196 -0.09143301]
      [-0.06327462  0.06911154 -0.99560392]]