卡尔曼滤波中如何将车速与车轮转角协方差转换为系统状态协方差

针对你遇到的协方差转换难题，核心是利用扩展卡尔曼滤波（EKF）处理自行车模型的非线性观测关系，直接将车速(v)和车轮转角(delta)作为测量值，无需强行将其协方差转换为状态维度的矩阵。以下是具体实现思路：

1. 明确状态、测量与观测函数 #

你的状态向量定义为：
X = [x, x_dot, x_2dot, y, y_dot, y_2dot, theta, theta_dot]^T

CAN总线提供的测量向量为：
Z = [v, delta]^T

基于简化自行车模型，测量值与状态变量的非线性观测函数为：

• 车速：v = x_dot * cos(theta) + y_dot * sin(theta)
• 车轮转角：delta = arctan( (L * theta_dot) / v )，其中L为车辆轴距

2. 构建观测雅可比矩阵H（EKF核心） #

由于观测函数是非线性的，需要计算雅可比矩阵H（2×8维度），将状态变量的微小变化映射到测量值的变化。H的每个元素是测量函数对状态变量的偏导数：

对车速v的偏导数（H的第一行） #

• ∂v/∂x = 0
• ∂v/∂x_dot = cos(theta)
• ∂v/∂x_2dot = 0
• ∂v/∂y = 0
• ∂v/∂y_dot = sin(theta)
• ∂v/∂y_2dot = 0
• ∂v/∂theta = -x_dot*sin(theta) + y_dot*cos(theta)
• ∂v/∂theta_dot = 0

对车轮转角delta的偏导数（H的第二行） #

记v = x_dot*cos(theta) + y_dot*sin(theta)，则：

• ∂delta/∂x = 0
• ∂delta/∂x_dot = - (L*theta_dot*sin(theta)) / (v² + (L*theta_dot)²)
• ∂delta/∂x_2dot = 0
• ∂delta/∂y = 0
• ∂delta/∂y_dot = - (L*theta_dot*cos(theta)) / (v² + (L*theta_dot)²)
• ∂delta/∂y_2dot = 0
• ∂delta/∂theta = (L*theta_dot*(x_dot*sin(theta) - y_dot*cos(theta))) / (v² + (L*theta_dot)²)
• ∂delta/∂theta_dot = (L*v) / (v² + (L*theta_dot)²)

3. 卡尔曼滤波更新步骤 #

直接使用CAN给出的2×2车速-车轮转角协方差矩阵作为测量噪声矩阵R，代入EKF更新流程：

1. 计算卡尔曼增益：K = P * H^T * (H*P*H^T + R)^-1
2. 状态更新：X = X + K*(Z - h(X))，其中h(X)是观测函数计算的预测测量值
3. 协方差更新：P = (I - K*H)*P，其中I为8×8单位矩阵

4. 关于两种思路的分析 #

• 第一种思路的问题：车速和车轮转角无法唯一确定你定义的6个（或8个）状态变量（比如x/y位置、x_2dot/y_2dot无法由v和delta推导），强行转换协方差到状态维度会引入无意义的约束，导致滤波结果失真。
• 第二种思路的修正：你之前的方向是对的，但需要用EKF处理非线性观测关系，而不是试图构建线性观测矩阵——因为自行车模型中v、delta与状态变量的关系是非线性的，线性观测矩阵无法准确描述。

额外补充：IMU数据的融合 #

IMU的线加速度（转换到全局坐标系）可直接对应x_2dot和y_2dot，角速度直接对应theta_dot。你可以将IMU和CAN的测量值合并为一个5维测量向量，构建分块对角的R矩阵（IMU与CAN噪声独立），再扩展雅可比矩阵H的维度即可。

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