加速度计与陀螺仪数据融合

G0[0,0,1]T

在传感器坐标中原重力加速度方向为：

Gg[gx,gy,gy]T（方向可归一化）

222归一化条件：gxgygz1.................................................................(1)

在传感器坐标中加速度为：

Ga[ax,ay,az]T（注意不能归一化，要有大值）

在传感器中真实重力加速度方向为：

Gb[bx,by,bz]T（方向可归一化）

归一化条件：bxbybz1..................................................................(2)

222Ga和Gg的转轴为GaGg

iGaGgaxgxjaygykaz(aygzazgy)i(azgxaxgz)j(axgyaygx)k.............（3）

gz修正量最小需满足的条件是

GaGg//GaGb（4）

[(aygzazgy)i(azgxaxgz)j(axgyaygx)k]//[(aybzazby)i(azbxaxbz)j(axbyaybx)k]（5）

(aygzazgy)k(aybzazby)(azgxaxgz)k(azbxaxbz)...........................................................(6) (agag)k(abab)yxxyyxxy加速度向真实加速方向投影

GaGaGbGb.........................................................................(8) Gb2所以得到

GaGbGbGa......................................................................(9)

2bx2bybz2axbxaybyazbz在新坐标中

aaa2x2y2z..........................................(10)

GbTcgG0

bxsin()2(Pb1Pb3bPb2))sin()2(Pb2Pb3bPb1)..................................(11) bycos(2222bcos()cos()(PPPbb3b1b2)z在原来坐标中

GgTG0

gx2(P1Pb3Pb2) gy2(P2P3P1)22221P2)gz(P3Ptan()P3P1P212(P2P3)22bPb3Pb1Pb212(Pb2Pb3)22..............................(12)

sin1(b)xby1.................................................(13) tan()bzP3P1P21)tan(2212(P2P3)

这里并不能求解出Tcg。通过实验发现其中方向角是比较稳定的，因此这里可以不改变方向角。利用T求出方向角，利用Tcg求出横滚角和俯仰角。通过三个角合成求出新的四元素。

bcos(2)cos(2)cos(2)sin(2)sin(2)sin(2)Pb1sin(2)cos(2)cos(2)cos(2)sin(2)sin(2)................................(14) Pb2cos(2)sin(2)cos(2)sin(2)cos(2)sin(2)Pb3cos(2)cos(2)sin(2)sin(2)sin(2)cos(2)