大家好，我正在做一个项目，需要用卡尔曼滤波器把陀螺仪数据（测量旋转增量）和AoA（到达角）测量数据（绝对角度，单位为度）融合起来，目的是估算物体的绝对朝向。

不过滤波器的结果总是出现发散或者异常的情况，我怀疑问题根源在于这两种测量类型的本质差异：

• 陀螺仪：测量的是旋转速度（随时间的变化量）
• AoA：提供绝对角度测量值（比如20度）

下面是我目前使用的卡尔曼滤波器实现代码：

def predict(posterior, angular_velocity, delta_t):
    x, P = posterior            # mean and variance of posterior
    dx, Q = angular_velocity    # mean and variance of angular velocity
    x = x + (dx * delta_t)
    P = P + Q * delta_t**2
    return (x, P)

def update(prior, measurement):
    x, P = prior         # mean and variance of prior
    z, R = measurement   # mean and variance of measurement
    
    y = z - x            # residual
    K = P / (P + R)      # Kalman gain
    x = x + K * y        # posterior mean
    P = (1 - K) * P      # posterior variance
    return (x, P)

# Standard deviations (adjust if needed)
gyroscope_std = 0.013   # Estimated 0.013 dps
aoa_std = 10.27         # Estimated 10.27 deg

# Initial state from the first measurement
start = 0
initial_angle = df.loc[start, 'theta_deg']
x = (initial_angle, aoa_std**2)  # (mean, variance)

predictions = []
timestamp = df.loc[0, 'timestamp']

for i, row in df[start:].iterrows():
    if i == 0:
        # Initial prediction is just the initial state
        predictions.append(x[0])
        continue
    
    delta_t = row['timestamp'] - timestamp
    timestamp = row['timestamp']
    angular_velocity = row['gyroscope_dps']
    aoa = row['theta_deg']
    
    # Use the gyroscope reading as the process model input
    gyro_reading = (angular_velocity, gyroscope_std**2)
    prior = predict(x, gyro_reading, delta_t)
    
    # Update with the AoA measurement
    x = update(prior, (aoa, aoa_std**2))
    
    predictions.append(x[0])

希望各位能帮我排查一下问题出在哪里，谢谢大家！

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