符号说明：

T:本次测量值跟上次测量值的间隔时间;

Restx(n-1)、Resty(n-1)、Restz(n-1)：第n-1次的位置的最优值向量；

GRatexz(n)，GRateyz(n)：陀螺仪的值，单独进行滤波后第n次的值。

GRx(n)、GRy(n)、GRz(n)：融合了陀螺仪的位置向量   

RAx(n)，RAy(n)，RAz(n)：第n次的加速度计的测量值向量；

Restx(n)、Resty(n)、Restz(n)：第n次的位置的最优值向量；

1、得到上次的位置最优值并归一化：Restx(n-1)，Resty(n-1)，Restz(n-1)。

其中：Restx(n-1)*Restx(n-1)+Resty(n-1)*Resty(n-1)+Restz(n-1)*Restz(n-1) = 1*1   

2、计算角度，根据Restx(n-1)，Resty(n-1)，Restz(n-1)计算出∠Axz、∠Ayz;

3、对陀螺仪的值进行归一化，得到GRatexz(n)，GRateyz(n);

4、数据融合一，把陀螺仪的数据融合进去：

　　∠Aestxz = ∠Axz + GRatexz(n) * T;

　　∠Aestyz = ∠Ayz + GRateyz(n) * T;

注：GRatexz和GRateyz最好是根据本次测量值和上次测量值进行处理得到的，比如取平均值。

5、步骤2的逆运算，得到GRx(n)、GRy(n)、GRz(n)：

　　①GRx(n)*GRx(n)+GRy(n)*GRy(n)+GRz(n)*GRz(n) = 1*1 

　　②sin∠Aestxz = GRx(n) / sqrt[GRx(n)*GRx(n) + GRz(n)*GRz(n)]

　　③sin∠Aestyz =GRy(n) / sqrt[GRy(n)*GRy(n) + GRz(n)*GRz(n)]

求出GRx(n)、GRy(n)、GRz(n);

6、数据融合二，数据滤波：

 　　(1)得到最新的加速度计的测量值，并归一化：RAx(n)，RAy(n)，RAz(n)。

其中：RAx(n)*RAx(n)+RAy(n)*RAy(n)+RAz(n)*RAz(n) = 1*1   

　　(2)根据GRx(n)、GRy(n)、GRz(n)和Rax(n)，Ray(n)，Raz(n)，进行数据处理，得到：Restx(n)、Resty(n)、Restz(n)。

至于怎么数据处理，有简单的加权平均，有复杂的卡尔曼滤波，笔者小白，只会加权平均滤波，就不献丑了。

7、重复1~7，进行下一轮的计算。

陀螺仪和加速度计融合算法流程