基于运动学自行车模型的扩展卡尔曼滤波器实现疑问

我正在实现扩展卡尔曼滤波器（EKF）以融合GPS与IMU传感器数据，选用运动学自行车模型作为状态动力学模型，推导的方程如下：

状态向量包含x位置、y位置、航向角；控制向量包含x向加速度、y向加速度、转向角。

import math
import numpy as np

def f(x,u): 
    # x: state vector [pos_x, posy, headingAngle] 
    # u: control vector [acc_x, acc_y, steerAngle] 
    dt = 1 # time step 
    wheelBaseLength = 2.5  # 示例轴距，需替换为实际值
    rearwheelBase = 1.0    # 示例后轮距相关参数，需替换为实际值
    pos_x, posy, headingAngle = x 
    acc_x, acc_y, steerAngle = u 
    vel_curr_x = acc_x*dt 
    vel_curr_y = acc_y*dt 
    v_Curr = np.sqrt(vel_curr_x**2 + vel_curr_y**2) 
    # 修复原代码语法错误
    sideSlipAngle = math.atan2((math.tan(steerAngle) * rearwheelBase) / wheelBaseLength, 1)
    vel_X = v_Curr * math.cos(headingAngle + sideSlipAngle) 
    vel_Y = v_Curr * math.sin(headingAngle + sideSlipAngle) 
    rate_HeadingAngle = v_Curr * ((math.tan(steerAngle) * math.cos(sideSlipAngle)) / wheelBaseLength)
    # State Equations 
    new_pos_x = pos_x + vel_X*dt 
    new_posy = posy + vel_Y*dt 
    new_heading = headingAngle + rate_HeadingAngle * dt
    return np.array([new_pos_x, new_posy, new_heading])

目前我已计算出状态转移矩阵F（f(x,u)对状态向量x的雅可比矩阵），结果为3×3单位矩阵，但卡在计算控制矩阵B（f(x,u)对输入向量u的雅可比矩阵）。由于变量间存在依赖关系，求偏导后得到的方程十分复杂，难以适配标准EKF表达式x = Fx + Bu。

我的问题是：在EKF中，是否仅需对输入向量求状态方程的偏导即可？若不可行，正确计算雅可比矩阵的方法是什么？

问题解答 #

• 关于是否仅对输入求偏导
  是的，EKF中控制矩阵B的定义就是状态转移函数f(x,u)对控制向量u的雅可比矩阵，本质就是求偏导。你遇到的推导复杂问题是运动学自行车模型变量耦合性强导致的，属于正常情况。

• 简化计算的可行方案

  1. 数值雅可比近似：放弃手动符号推导，用数值方法计算。对控制向量的每个分量施加微小扰动Δu（比如1e-6量级），分别计算(f(x, u+Δu) - f(x,u))/Δu，得到的结果就是B矩阵的对应列。这种方法代码实现简单，精度足够满足EKF需求，无需处理复杂的符号运算。
  2. 扩展状态向量：当前状态向量只包含位置和航向，缺少速度变量。把状态扩展为[pos_x, pos_y, heading, vel_x, vel_y]，这样加速度直接作用于速度，速度再积分到位置，模型的耦合性会大幅降低，雅可比矩阵的推导会更清晰，也更符合实际运动规律。
  3. 链式法则分步推导：如果坚持符号推导，拆分中间变量（如v_Curr、sideSlipAngle等），用链式法则分步计算每个状态分量对u各分量的偏导，避免一次性推导整个复杂表达式，降低出错概率。

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