Abstract: 之前写了数学原理，在线标定要用到这个算法，找了些资料实现了一下，这是pitch和yaw角的ekf。

矩阵的定义

协方差，运动方程，观测方程等

# 状态协方差矩阵 P
self.P = np.diag([0.1, 0.1, 0.5, 0.5])

# 过程噪声协方差 Q
self.Q_base = np.diag([0.001, 0.001, 0.01, 0.01])

# 测量噪声协方差 R
self.R_base = np.diag([0.05**2, 0.03**2])  # sigma_pitch=0.005, sigma_yaw=0.003

# 状态转移矩阵 F
self.F = np.array([[1, 0, dt, 0],
                    [0, 1, 0, dt],
                    [0, 0, 1, 0],
                    [0, 0, 0, 1]])

# 观测矩阵 H
self.H = np.array([[1, 0, 0, 0],
                    [0, 1, 0, 0]])

预测步

与推导一致

def predict(self):
    """预测步"""
    self.x = self.F @ self.x
    self.P = self.F @ self.P @ self.F.T + self.Q_base
    return self.x.copy()

更新

def update(self, z_meas, confidence=1.0):
    """
    更新步
    """
    # 计算新息
    z_pred = self.H @ self.x
    # y是先验观测值减掉通过运动方程转换来的先验状态值
    y = np.array([z_meas[0] - z_pred[0], z_meas[1] - z_pred[1]])
    
    # 角度包裹处理
    y[0] = self.normalize_angle(y[0])
    y[1] = self.normalize_angle(y[1])
    
    # 自适应噪声调整
    R = self.R_base / confidence if self.use_adaptive else self.R_base
    
    # 计算新息协方差
    S = self.H @ self.P @ self.H.T + R
    
    # 卡尔曼增益
    K = self.P @ self.H.T @ np.linalg.inv(S)
    
    # 状态更新
    self.x = self.x + K @ y
    
    # 协方差更新
    I = np.eye(4)
    self.P = (I - K @ self.H) @ self.P @ (I - K @ self.H).T + K @ R @ K.T

模拟

输入十帧的模拟数据：

timestamps = np.array([0.0, 0.033, 0.067, 0.1, 0.133, 0.167, 0.2, 0.233, 0.267, 0.3])

# 观测到的pitch和yaw（单位：弧度），带有噪声
pitch_obs = np.array([0.05, 0.048, 0.045, 0.042, 0.04, 0.038, 0.037, 0.036, 0.035, 0.034])
yaw_obs = np.array([-0.01, -0.008, -0.005, -0.003, 0.0, 0.002, 0.003, 0.004, 0.005, 0.006])

输出：

Last modified on 2026-03-18