常速卡尔曼滤波器状态约束的需求与实现技术问询

1. 硬约束的施加时机 #

硬约束Dx=d应当在卡尔曼滤波的更新步骤完成后，施加于当前时刻的后验状态向量。

卡尔曼滤波的预测、更新步骤本身是无约束的状态估计过程，更新得到的后验状态可能偏离预设的常速动力学关系。此时施加硬约束，能强制后验状态严格满足(x_k^+ - x_{k-1}^+)/dt = v_{x,k}^+和(y_k^+ - y_{k-1}^+)/dt = v_{y,k}^+的运动规则，让状态完全贴合常速模型。

2. D矩阵的正确性分析 #

你设计的D矩阵结构是正确的，不需要用dx、dy替代状态向量中的x、y，但需要修正d向量：

• 你给出的D矩阵形式：

D = | 1  0 -dt  0 |
    | 0  1  0  -dt|

这个矩阵的结构准确对应了约束方程的系数。

• 但你设定的d向量[0,0]^T是错误的：你想要实现的约束是当前后验位置与上一时刻后验位置的差值和速度匹配，整理后约束方程应为：
  x_k^+ - dt*v_{x,k}^+ = x_{k-1}^+
y_k^+ - dt*v_{y,k}^+ = y_{k-1}^+
  因此d向量应该是上一时刻的后验位置，即d = [x_{k-1}^+, y_{k-1}^+]^T，而非全零向量。如果用全零向量，约束会变成x_k^+ = dt*v_{x,k}^+，这完全不符合你想要的动力学逻辑。

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