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

在地面坐标中重力加速度方向为TG]1,0,0[0在传感器坐标中原重力加速度方向为：TyyxggggG],,[（方向可归一化）归一化条件：1222zyxggg.................................................................(1)在传感器坐标中加速度为：TzyxaaaaG],,[（注意不能归一化，要有大小参考值）在传感器中真实重力加速度方向为：TzyxbbbbG],,[（方向可归一化）归一化条件：1222zyxbbb..................................................................(2)gaGG和的转轴为gaGGkgagajgagaigagagggaaakjiGGxyyxzxxzyzzyzyxzyxga)()()(.............（3）修正量最小需满足的条件是bagaGGGG//（4）])()()//[(])()()[(kbabajbabaibabakgagajgagaigagaxyyxzxxzyzzyxyyxzxxzyzzy（5）)()()()()()(xyyxxyyxzxxzzxxzyzzyyzzybabakgagababakgagababakgaga...........................................................(6)加速度向真实加速方向投影bbbaaGGGGG.........................................................................(8)所以得到abbaGGGG2......................................................................(9)222222zyxzyxzzyyxxaaabbbbababa..........................................(10)在新坐标中0GTGcgb)()cos()cos()(2)sin()cos()(2)sin(2221232132231bbbbzbbbbybbbbxPPPbPPPbPPPb..................................(11)在原来坐标中0TGGg)()(2)(22221232132231PPPgPPPgPPPgzybbx)(21)(21)tan(23222132322213bbbbbbPPPPPPPPPP..............................(12)))(21(tan)(tan)(sin2322213111PPPPPbbbzyx.................................................(13)这里并不能求解出cgT。通过实验发现其中方向角是比较稳定的，因此这里可以不改变方向角。利用T求出方向角，利用cgT求出横滚角和俯仰角。通过三个角合成求出新的四元素。)cos()sin()sin()sin()cos()cos()sin()cos()sin()cos()sin()cos()sin()sin()cos()cos()cos()sin()sin()sin()sin()cos()cos()cos(222222322222222222221222222bbbbPPP................................(14)