IMU传感器UKF未利用角速度与线加速度数据问题求助

各位大佬好，我现在碰到个棘手的问题，想请大家帮忙看看：

我基于Roger R Labbe Jr的《Kalman and Bayesian Filters in Python》实现了一个针对IMU传感器的UKF，依赖只有FilterPy，按理直接就能运行。但奇怪的是，这个滤波程序虽然用到了IMU的姿态（orientation）数据，但完全没用到角速度和线加速度——不管这两个数据怎么调整，滤波结果都没有任何变化。另外我还预留了GPS和地标传感器的数据（现在是注释状态），只要取消注释，滤波就能正常利用这些数据，唯独IMU的角速度和线加速度完全不起作用，有没有大佬能帮我定位下问题？

附上我的代码：

# Based on Kalman and Bayesian Filters in Python, Roger R Labbe Jr
# Use: MIT license

import numpy as np
from numpy.random import randn

import copy

import scipy.stats as stats
from scipy.linalg import block_diag

from math import tan, sin, cos, sqrt
from math import cos, sin, atan2, pi

import matplotlib.pyplot as plt

from filterpy.kalman import UnscentedKalmanFilter as UKF
from filterpy.common import Q_discrete_white_noise
from filterpy.kalman import JulierSigmaPoints
from filterpy.kalman import unscented_transform, MerweScaledSigmaPoints
from filterpy.stats import ...

我自己初步排查过的点（供参考）： #

• 程序能正常运行，没有报错
• GPS和地标数据取消注释后能正常参与滤波，说明UKF的基本框架是没问题的
• IMU的姿态数据是能正常影响结果的，唯独角速度和线加速度完全无效

可能的排查方向（欢迎补充）： #

• 检查状态转移函数：UKF的状态转移过程是不是没把角速度和线加速度纳入计算？比如姿态更新应该用角速度积分，速度/位置更新应该用线加速度积分，如果这部分逻辑缺失，IMU的这两个数据自然没法发挥作用。
• 确认数据传入流程：有没有把角速度和线加速度作为参数传递给UKF的predict()或者update()函数？会不会是代码里根本没把这些数据喂进滤波流程？
• 核对状态向量定义：状态变量里有没有包含需要用IMU数据更新的分量（比如角速度偏差、加速度偏差、速度量、姿态角变化率）？如果状态里没有对应变量，IMU数据也找不到作用的对象。
• 检查噪声矩阵Q：是不是给IMU相关的过程噪声设得过大，导致数据的权重被压得极低？或者Q矩阵的维度、对应关系没匹配状态里的IMU相关变量？
• 确认传感器融合逻辑：IMU的角速度和线加速度一般是在predict阶段用来预测状态的，而不是update阶段（除非是直接观测角速度/加速度），是不是把数据放错了处理阶段？

备注：内容来源于stack exchange，提问作者Steve Brown