过度校正卡尔曼滤波器

我正在尝试使扩展的卡尔曼滤波器正常工作。我的系统模型是：

$ x = \ begin {bmatrix}
lat \\
long \\
\ theta
\ end {bmatrix} $

其中lat和long是经度和纬度（以度为单位），而$ \ theta $是车辆的当前方向（也以度为单位）。
在我的预测步骤中，我得到了读取当前速度v，偏航率$ \ omega $和倾斜角度$ \ alpha $：

$ z = \ begin {bmatrix}
v \\
\ alpha \ \
\ omega
\ end {bmatrix} $

我对EKF使用标准预测，其中$ f（）$为：

$
\ vec {f}（\ vec {x} _ {u，t}，\ vec {z} _t）= \ vec {x} _ {u，t} +
\ begin {bmatrix }
\ frac {v} {f} * \ cos（\ theta）* \ cos（\ alpha）* \ frac {180°} {\ pi * R_0} \\
\ frac {v } {f} * \ sin（\ theta）* \ cos（\ alpha）* \ frac {180°} {\ pi * R_0} * \ frac {1} {\ cos（lat）} \\
\ frac {\ omega} {f}
\ end {bmatrix}
$

$ f $是预测频率，$ R_0 $是地球半径（建模地球作为一个球体）

我的雅可比矩阵看起来像这样：

$
C = v \ cdot \ Delta t \ cdot cos（\ alpha）\ cdot \ frac {180} {\ pi R_0}
$

$
F_J =
\ begin {pmatrix}
1＆0＆-C \ cdot sin（\ phi）\ cdot \ frac {1} {cos（ lat）} \\
-C \ cdot sin（\ phi）\ cdot \ frac {sin（lat）} {{cos（lat）} ^ 2}＆1＆C \ cdot cos（\ phi）\ cdot \ frac {1} {cos（lat）} \\
0＆0＆1
\ end {pmatrix}
$

在进行预测步骤时，传感器的频率较高，我进行了约10次预测，然后进行了一次更新。和前一个。因此，我的更新步骤只是标准的EKF更新，其中$ h（x）= x $，因此将Jacobian矩阵转换为$ h（）$，其中$ H $为标识。

尝试使用测试数据来实现，其中GPS轨迹位于恒定的北方方向，并且偏航角不断向西旋转，我希望过滤器将我的位置靠近轨迹，并将方向校正到355度左右。实际发生的情况可以在所附的图像中看到（红色：GPS位置测量，绿色/蓝色：预测位置）：

我不知道该怎么办。我对卡尔曼滤波器的经验不是很丰富，所以可能是我误会了一些东西，但是我尝试过的一切似乎都没有效果……

我的想法：

我戳了一下大概：如果我将预测中的雅可比矩阵设置为恒等式，则效果很好。问题似乎是$ P $（系统模型的协方差矩阵）在$ P（3,1）$和$ P（3,2）$中不为零。我的解释是，在预测步骤中，方向取决于位置，这似乎没有任何意义。这是由于$ F_J（2,1）$不为零，这又是有道理的。

谁能给我一个提示，指出过度校正可能来自哪里，或者我应该看一下/ google for？