Filter 我的陀螺仪MPU6050 GY521的Z轴（偏航）未正确输出卡尔曼滤波数据

为什么我的陀螺仪MPU6050 GY521的Z轴（偏航）没有正确输出卡尔曼滤波数据？我从库中得到了这个草图，并对它进行了调整，以添加过滤后的Z数据，因为原始的只有X和Y轴

我不确定哪里出了问题，请有人帮我找出要纠正或省略的地方

#包括
#包括//来源：https://github.com/TKJElectronics/KalmanFilter
#定义音高
Kalman-kalmanX；//创建Kalman实例
Kalman-Kalman；
Kalman-kalmanZ；
/*IMU数据*/
双accX、accY、accZ；
双陀螺X、陀螺Y、陀螺Z；
int16_t tempRaw；
双回旋角、回旋角、回旋角；//仅使用陀螺仪计算角度
双压扩X、压扩Y、压扩Z；//使用互补滤波器计算角度
双三角帆，三角帆，三角帆；//使用卡尔曼滤波器计算角度
uint32_t定时器；
uint8_t i2Cata[14]；//I2C数据缓冲区
int XPin=A0；
int-YPin=A1；
int自旋=2；
int XVal、YVal、SVal；
intjx，Jy，Js；
//TODO:执行校准例行程序
无效设置（）{
序列号开始（115200）；
Wire.begin（）；
pinMode（XPin，输入）；
pinMode（输入类型输入）；
pinMode（旋转、输入）；
数字写入（自旋，高）；
#如果ARDUINO>=157
Wire.setClock（400000UL）；//将I2C频率设置为400kHz
#否则
TWBR=（（F_CPU/400000UL）-16）/2；//将I2C频率设置为400kHz
#恩迪夫
i2cData[0]=7；//将采样率设置为1000Hz-8kHz/（7+1）=1000Hz
i2cData[1]=0x00；//禁用FSYNC并设置260 Hz Acc滤波、256 Hz陀螺滤波、8 KHz采样
i2cData[2]=0x00；//将陀螺满标度范围设置为±250度/秒
i2cData[3]=0x00；//将加速计满刻度范围设置为±2g
while（i2cWrite（0x19，i2cData，4，false））；//一次写入所有四个寄存器
while（i2cWrite（0x6B，0x01，true））；//带X轴陀螺仪参考的PLL，禁用休眠模式
而（i2cRead（0x75，i2cData，1））；
如果（i2cData[0]！=0x68）{//读取“谁是我”寄存器
串行打印（F（“错误读取传感器”）；
而(1),；
}
延迟（100）；//等待传感器稳定
/*设置kalman和陀螺仪起始角*/
而（i2cRead（0x3B，i2cData，6））；
accX=（int16_t）（（i2cData[0]180）
gyroYangle=kalAngleY；
如果（陀螺赞格勒<-180 | |陀螺赞格勒>180）
Gyrosangle=kalAngleZ；
/*打印数据*/
#如果0//设置为1以激活
串行打印（accX）；串行打印（“\t”）；
串行打印（accY）；串行打印（“\t”）；
串行打印（accZ）；串行打印（“\t”）；
串行打印（gyroX）；串行打印（“\t”）；
串行打印（陀螺）；串行打印（“\t”）；
Serial.print（gyroZ）；Serial.print（“\t”）；
串行打印（“\t”）；
#恩迪夫
Serial.print（Ax:）；Serial.println（accX）；
Serial.print（“Ay:”）；Serial.println（accY）；
Serial.print（“Az:”）；Serial.println（accZ）；
//串行打印（滚动）；串行打印（“\t”）；
//Serial.print（gyroXangle）；Serial.print（“\t”）；
//Serial.print（compAngleX）；Serial.print（“\t”）；
Serial.print（“X:”；Serial.println（kalAngleX）；//Serial.print（“\t”）；
//串行打印（“\t”）；
//串行打印（音高）；串行打印（“\t”）；
//Serial.print（gyroYangle）；Serial.print（“\t”）；
//Serial.print（公司）；Serial.print（“\t”）；
Serial.print（“Y:”；Serial.println（kalAngleY）；//Serial.print（“\t”）；
Serial.print（“Z:”；Serial.println（kalAngleZ）；
#如果0//设置为1，则打印温度
串行打印（“\t”）；
双温度=（双）温度/340.0+36.53；
串行打印（温度）；串行打印（“\t”）；
#恩迪夫
//串行打印（“\r\n”）；
延迟（2）；
}