卡尔曼滤波器GPS + IMU融合通过低成本传感器获得准确的速度

我是所有这些机器人技术的新手。特别是对卡尔曼滤波器。

我的最初目标是使速度尽可能精确

这是我的情况：


GPS gives me:


位置（经度，纬度，高度）
位置精度（误差不能分为east, north, up方向）
速度
速度精度（误差不能分为east, north, up方向）
航向角
航向角精度

IMU: (separated accelerometer, gyroscope and magnetometer). I fuse them myself

实际上，需要指出的是，我不能使用磁力计。由于“汽车”是法拉第笼。所以我只能将加速度计和陀螺仪融合在一起。它们都是Madgwick AHRS的输出（如果需要，可以从此处获得旋转矩阵，四元数），并以North，East和Up尺寸表示。

到目前为止，我所做的是：


我已经实现了具有位置和速度状态的线性KF。但是我没有达到期望的精度。



摆脱上图的IMU数据。 />
是由IMU引起的。我每1秒钟更新一次GPS。和具有13 Hz频率的IMU。我们在这里可以看到，每进行13次迭代，我们就会更新GPS，然后IMU就会流氓。但是我的情况是13Hz。由于不需要太多更新。


预测IMU触发事件的时间
GPS触发事件的时间。我获取最新的IMU数据。先预测然后更新gps（位置，速度）。

因为我的主要目标是获得准确的速度。
我不太在乎位置和航向角，但是...速度与它们相关，可以将它们添加到卡尔曼滤波器。

我的卡尔曼状态是position，velocity和heading angle。

我可以使用类似的东西吗？

$$
x = x_i + v_i \ Delta t + \ frac {a_i \ Delta t} {2}
$$
$$
v = v_i + a_i \ Delta t
$$
$$
\ theta = \ theta_i + w_i \ Delta t
$$
问题：


将位置和航向角作为卡尔曼滤波器的状态可以使速度受益。由于它们之间存在某种关联。 （角速度会影响速度本身）。
使用线性运动公式是否可以？因为我的情况下有曲线运动。
几乎所有论文都用KF描述（位置，速度）模型。我可以利用EKF优势吗？我发现了一些提到里程表字的论文。似乎它们具有相同的模型。 （位置，速度，角度）
如果在所有操作之后速度仍然不准确怎么办？我应该在KF之后再使用其他工具吗？
我可以以某种方式利用当前位置和上一步吗？定位点？ （例如，从两个点计算速度。当然，这意味着我的单位线性移动，而不是曲线移动）。然后以这种速度以某种方式校正我的预测KF结果。


请帮助我建模Kalman滤波器。并给出如何获得最佳速度精度的建议。


谢谢！