融合GPS、里程计与磁力计的EKF航向输出错误排查

我正在用扩展卡尔曼滤波器（EKF）估计配备GPS、磁力计和轮式里程计的2D移动机器人位姿。参考系设定为正北0°、角度顺时针递增，磁力计的XY轴切换问题已在代码中完成转换验证。滤波器原本要实现GPS采样插值，并通过调优测量噪声矩阵优化GPS不确定性，但从放大图可见航向计算结果错误，需要排查滤波器定义中的错误并给出修复方案。

class EKF(object):
    '''
    State: [x, y, vx, vy, h]
    '''
    def __init__(self, x, y, vx, vy, h):
        self.wheel_distance = 0.34 

        # State variables
        self.X = np.array([x, y, vx, vy, h])

        # Initial covariance
        self.P = np.eye(5)  

        # Process noise
        self.Q = np.eye(5)*0.1

        # Measurement noise
        self.R_gps = np.eye(2) * 5 
        self.R_mag = np.eye(1) * 0.1 
        self.R_enc = np.eye(2) * 0.1 

        # Measurement matrices
        self.H_gps = np.zeros((2, 5))
        self.H_gps[0, 0] = 1.0
        self.H_gps[1, 1] = 1.0

        self.H_mag = np.zeros((1, 5))
        self.H_mag[0, 4] = 1.0

        self.H_enc = np.zeros((2, 5))

    def update(self, vl, vr, gx, gy, mx, my, dt):

        # Prediction    
        A = np.eye(5)
        # x = x0 + vx
        A[0, 2] = dt
        # x = xy + vy
        A[1, 3] = dt
    
        self.X = np.dot(A, self.X)
        self.P = np.dot(np.dot(A, self.P), A.T) + self.Q

        # Magnetometer update
        K_mag = np.dot(np.dot(self.P, self.H_mag.T),
                                np.linalg.inv(np.dot(np.dot(self.H_mag, self.P), 
                                            self.H_mag.T) + self.R_mag))
        tmp = np.arctan2(mx, my) # measured h
        # convert to my reference system 
        if mx >= 0 and my >= 0:
            tmp = 2*np.pi - tmp
        elif mx >= 0 and my < 0:
            tmp = -tmp
        elif mx < 0 and my < 0:
            tmp = -tmp
        elif mx < 0 and my >= 0:
            tmp = 2*np.pi - tmp
        tmp += np.pi/2
        if tmp < 0:
            tmp += 2*np.pi
        elif tmp > 2*np.pi:
            tmp -= 2*np.pi
        
        Z_mag = np.array([tmp])
        innovation_mag = Z_mag - np.dot(self.H_mag, self.X)
        self.X = self.X + np.dot(K_mag, innovation_mag)     

        self.P = np.dot((np.eye(5) - np.dot(K_mag, self.H_mag)), self.P)

        # Encoder update
        
        dleft = vl*dt 
        dright = vr*dt 
        dcenter = (dleft + dright)/2
        w = (dleft - dright) / (2*self.wheel_distance)  
        vcenter = (vl + vr)/2
        theta = self.X[4] + w # TODO: w/2??
        x = self.X[0] + dcenter*np.sin(theta)
        y = self.X[1] + dcenter*np.cos(theta)
        vx = vcenter*np.sin(theta)
        vy = vcenter*np.cos(theta)  

        sin = np.sin(theta)
        cos = np.cos(theta)

        self.H_enc[0, 2] = 2/sin 
        self.H_enc[0, 3] = -2/cos
        self.H_enc[1, 2] = 2/sin
        self.H_enc[1, 3] = 2/cos
    
        K_enc = np.dot(np.dot(self.P, self.H_enc.T), 
                       np.linalg.inv(np.dot(np.dot(self.H_enc, self.P), 
                                            self.H_enc.T) + self.R_enc))
        Z_enc = np.array([vl, vr])
        innovation_enc = Z_enc - np.dot(self.H_enc, self.X)
        
        self.X = self.X + np.dot(K_enc, innovation_enc)
        self.P = np.dot((np.eye(5) - np.dot(K_enc, self.H_enc)), self.P)
        

        # GPS update
        
        # the GPS measurements have a lower sampling frequency
        if gx is not None:
            K_gps = np.dot(np.dot(self.P, self.H_gps.T), 
                           np.linalg.inv(np.dot(np.dot(self.H_gps, self.P), 
                                                self.H_gps.T) + self.R_gps))
            Z_gps = np.array([gx, gy])
            innovation_gps = Z_gps - np.dot(self.H_gps, self.X)
            
            
            self.X = self.X + np.dot(K_gps, innovation_gps)
            self.P = np.dot((np.eye(5) - np.dot(K_gps, self.H_gps)), self.P)
        
        # Wrap angle
        if self.X[4] < 0:
            self.X[4] += 2*np.pi
        if self.X[4] > 2*np.pi:
            self.X[4] -= 2*np.pi

        return self.X

• 预测阶段遗漏航向更新：原代码状态转移矩阵仅更新x、y，完全忽略航向h的变化，不符合差分驱动机器人的运动学模型，导致状态预测与实际运动严重偏离。
• 编码器观测模型完全错误：
  1. 角速度计算符号错误：w = (dleft - dright) / (2*self.wheel_distance)，差分驱动机器人顺时针转向时右轮速度大于左轮，正确符号应为(dright - dleft)。
  2. 航向增量计算错误：theta = self.X[4] + w未乘以时间步长dt，无法得到正确的航向变化量。
  3. H_enc矩阵推导无依据：当前H_enc元素与状态到轮速的观测逻辑完全不符，属于无效计算。
• 磁力计创新值未做角度环绕：角度差直接相减可能产生超过±π的跳变，导致滤波更新方向错误。

1. 重构预测阶段的状态转移 #

基于差分驱动机器人运动学模型，加入航向更新：

# Prediction
x, y, vx, vy, h = self.X
# 计算正确的角速度（顺时针为正）
w = (vr - vl) / self.wheel_distance

# 预测新状态
x_new = x + vx * dt
y_new = y + vy * dt
vx_new = vx
vy_new = vy
h_new = h + w * dt

# 构建状态转移雅可比矩阵
A = np.eye(5)
A[0, 2] = dt
A[1, 3] = dt

self.X = np.array([x_new, y_new, vx_new, vy_new, h_new])
self.P = np.dot(np.dot(A, self.P), A.T) + self.Q

2. 重新实现编码器更新逻辑 #

修正观测模型与H矩阵推导：

# Encoder update
x, y, vx, vy, h = self.X

# 从状态推导本体前进速度
v_body = vx * np.sin(h) + vy * np.cos(h)
# 从测量值计算角速度
w_body = (vr - vl) / self.wheel_distance

# 预测轮速
vl_pred = v_body - w_body * (self.wheel_distance / 2)
vr_pred = v_body + w_body * (self.wheel_distance / 2)
Z_enc_pred = np.array([vl_pred, vr_pred])

# 构建正确的雅可比矩阵H_enc
H_enc = np.zeros((2, 5))
# vl对状态的偏导
H_enc[0, 2] = np.sin(h)
H_enc[0, 3] = np.cos(h)
H_enc[0, 4] = vx * np.cos(h) - vy * np.sin(h)
# vr对状态的偏导
H_enc[1, 2] = np.sin(h)
H_enc[1, 3] = np.cos(h)
H_enc[1, 4] = vx * np.cos(h) - vy * np.sin(h)

# 卡尔曼增益计算与状态更新
K_enc = np.dot(np.dot(self.P, H_enc.T), 
               np.linalg.inv(np.dot(np.dot(H_enc, self.P), H_enc.T) + self.R_enc))
Z_enc = np.array([vl, vr])
innovation_enc = Z_enc - Z_enc_pred

self.X = self.X + np.dot(K_enc, innovation_enc)
self.P = np.dot((np.eye(5) - np.dot(K_enc, H_enc)), self.P)

3. 磁力计创新值角度环绕处理 #

# Magnetometer update
# ... 原有角度转换代码保持不变 ...
Z_mag = np.array([tmp])
innovation_mag = Z_mag - np.dot(self.H_mag, self.X)
# 将角度差限制在[-π, π]范围内
innovation_mag = np.arctan2(np.sin(innovation_mag), np.cos(innovation_mag))

self.X = self.X + np.dot(K_mag, innovation_mag)     
self.P = np.dot((np.eye(5) - np.dot(K_mag, self.H_mag)), self.P)

4. 优化噪声矩阵初始化 #

根据实际传感器精度调整噪声：

# 过程噪声：位置噪声最小，速度次之，航向噪声稍大
self.Q = np.diag([0.01, 0.01, 0.05, 0.05, 0.1])
# GPS噪声：根据实际GPS精度调整，建议1~2
self.R_gps = np.eye(2) * 1.5

• 实现GPS采样插值：在GPS无新数据时，用预测值或历史观测约束填充，避免滤波间隙发散。
• 验证磁力计角度转换：打印转换后的角度与机器人实际朝向，确认转换逻辑正确。

内容的提问来源于stack exchange，提问作者firion