符号说明：
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，进行下一轮的计算。