扩展卡尔曼滤波(EKF)扩展状态估计测量偏差技术咨询

嘿，你这个思路完全没问题——把测量偏差增广为状态量是EKF里处理系统测量偏差的标准操作，我来给你捋捋具体实现时要注意的几个关键点：

1. 状态向量的扩展 #

原来的6维状态向量是：

X = [x, y, z, vx, vy, vz]^T

现在新增两个测量偏差状态，对应两个角度测量的偏差，扩展为8维：

X = [x, y, z, vx, vy, vz, b₁, b₂]^T

其中b₁是第一个角度测量的偏差，b₂是第二个角度测量的偏差。

2. 状态转移方程与过程噪声的调整 #

通常我们假设测量偏差是恒定值或者慢变漂移量：

• 如果是恒定偏差，状态转移方程里偏差的更新就是b₁(k+1) = b₁(k)、b₂(k+1) = b₂(k)，对应的状态转移矩阵F要扩展为8×8的形式：

  F = [ 6×6运动学转移矩阵,  0  0;
        0 0 0 0 0 0,       1  0;
        0 0 0 0 0 0,       0  1 ]
  

• 如果考虑偏差有缓慢漂移，可以在过程噪声协方差Q中，给b₁和b₂对应的对角线元素设置一个很小的值（比如1e-8，具体数值根据你的仿真场景调整），模拟偏差的微小变化。

3. 测量方程与雅可比矩阵的修改 #

原测量方程是基于真实角度计算的，现在测量值是真实角度+偏差，所以测量模型要更新为：

预测测量值 = 基于当前状态估计的真实角度 + 偏差状态的估计值

用公式表示就是：

h(X) = [ h₁(X[0:5]) + X[6],  h₂(X[0:5]) + X[7] ]^T

其中h₁、h₂是原来的角度测量模型（比如从3D位置计算方位角、俯仰角的函数）。

对应的测量雅可比矩阵H也要扩展为2×8的形式：

H = [ ∂h₁/∂x  ∂h₁/∂y  ∂h₁/∂z  ∂h₁/∂vx  ∂h₁/∂vy  ∂h₁/∂vz  1  0;
      ∂h₂/∂x  ∂h₂/∂y  ∂h₂/∂z  ∂h₂/∂vx  ∂h₂/∂vy  ∂h₂/∂vz  0  1 ]

这里前6列是原来的雅可比矩阵，最后两列对应偏差状态的偏导——因为h₁对b₁的偏导是1，对b₂是0，反之亦然。

4. 初始值与协方差的设置 #

• 偏差初始估计：如果没有先验信息，b₁、b₂的初始值可以设为0；如果知道偏差的大致范围，也可以设为对应的值。
• 初始协方差P₀：对应b₁、b₂的对角线元素要设得大一些（比如1e2，单位匹配你的角度单位：度或角秒），表示初始对偏差的不确定性很高。
• 过程噪声Q：如果是恒定偏差，偏差对应的元素设为0；如果是慢变偏差，设一个极小的正数即可。

5. 仿真验证的关键步骤 #

• 先跑通原来的6维EKF，确保在无偏差的情况下位置、速度估计正常；
• 给测量值添加已知的偏差（比如1度或1角秒），运行8维EKF，观察b₁、b₂的估计值是否收敛到你设定的真实偏差；
• 重点检查位置和速度的估计精度——如果添加偏差估计后，位置速度的估计误差变小了，就说明你的实现是正确的。

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