MPU3050数据：三个方向角速度

确定精度：

(17条消息) 陀螺仪和加速度计MPU6050的单位换算方法_lgcjlu的博客-CSDN博客_mpu6050加速度

参数确定：

AccX,Accy,AccZ：加速度计三个方向加速度

GyroX,GyroY,GyroZ：陀螺仪三个方向角速度

accAngleX，accAngleY ：由加速度计解算的角度值

gyroAngleX,gyroAngleY：由陀螺仪解算的角度值

roll,pitch,yaw：滚转，俯仰，偏航角

elapsedTime：时间差（大小由循环程序的频率决定）

主要公式：

accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.58;

accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.58;

gyroAngleX = gyroAngleX + GyroX * elapsedTime;

gyroAngleY = gyroAngleY + GyroY * elapsedTime;（对角速度积分，注意初值的设置和单位问题）

roll = GYRO_WEIGHT * gyroAngleX + ACC_WEIGHT * accAngleX;

pitch = GYRO_WEIGHT * gyroAngleY + GYRO_WEIGHT * accAngleY;

此处的权重可适当调整，实现互补滤波的作用，消除陀螺仪的漂移。网上给出的是0.96、0.04

yaw =  yaw + GyroZ * elapsedTime;

注意：由于无法从加速度计计算偏航角，所以yaw直接由陀螺仪得出（本次实验中也不会用到偏航角）

主要思路：

1. 求取初始纠错值

获得初始时刻的误差：保持在一个平坦静止的初始位置，计算一定数量的初始值（示例中为200次），求平均值即为误差（理论上应该是0）

2. 读取数据

每一次读取数据（角速度或加速度）后都要先进行一次纠错

3. 按照公式计算出当前的位姿