kalman-filter - 惯性导航系统的间接卡尔曼滤波器

Question

我正在尝试使用间接卡尔曼滤波器 实现惯性导航系统。我发现了很多关于这个主题的出版物和论文，但没有太多的代码作为例子。对于我的实现，我使用以下链接提供的硕士论文:

https://fenix.tecnico.ulisboa.pt/downloadFile/395137332405/dissertacao.pdf

如第 47 页所述，惯性传感器的测量值等于真实值加上一系列其他项(偏差、比例因子等)。对于我的问题，让我们只考虑偏见。

所以:

Wmeas = Wtrue + BiasW   (Gyro meas)
Ameas = Atrue + BiasA.  (Accelerometer meas)

因此，

当我传播机械化方程时(方程 3-29、3-37 和 3-41)我应该使用“真实”值，或者更好:

Wmeas - BiasW
Ameas - BiasA

其中 BiasW 和 BiasA 是偏差的最后可用估计值。对吧？

关于 EKF 的更新阶段，如果测量方程是

dzV = VelGPS_est - VelGPS_meas

H 矩阵应该有一个单位矩阵对应于速度误差状态变量 dx(VEL) 和其他地方的 0。对吧？

说我不确定在更新阶段后我必须如何传播状态变量。状态变量的传播应该是(在我看来):

POSk|k = POSk|k-1 + dx(POS);
VELk|k = VELk|k-1 + dx(VEL);
...

但这没有用。因此我尝试过:

POSk|k = POSk|k-1 - dx(POS);
VELk|k = VELk|k-1 - dx(VEL);

那也没用...我尝试了两种解决方案，即使在我看来应该使用“+”。但是因为两者都不起作用(我在其他地方还有一些其他错误)我会问你是否有任何建议。

您可以在以下链接中查看代码片段:http://pastebin.com/aGhKh2ck .

谢谢。

Answer 1

您遇到的困难是理论与实践之间的差异。从代码片段而不是问题中的符号版本中获取代码:

    % Apply corrections
    Pned = Pned + dx(1:3);
    Vned = Vned + dx(4:6);

在 理论 中，当您使用间接形式时，您可以自由地集成 IMU(该过程在该论文中称为机械化)并偶尔运行 IKF 以更新其校正。在理论中，加速度计未经检查的双重积分会在 Pned 和 Vned 中产生较大(或者对于便宜的 MEMS IMU，巨大)误差值。这反过来又会导致 IKF 随着时间的推移产生相应较大的 dx(1:6) 值，并且未经检查的 IMU 集成越来越偏离事实。在 理论 中，您可以随时将您的位置采样为 Pned +/- dx(1:3)(符号并不重要 - 您可以设置它方法)。这里的重要部分是您没有修改 IKF 中的 Pned，因为它们彼此独立运行，并且在您需要答案时将它们添加在一起。

在实践中，您不想取两个巨大的double 值之间的差值，因为您会失去精度(因为需要有效数的许多位来表示巨大的部分而不是你想要的精度)。您已经了解，在实践 中，您希望在每次更新时递归更新 Pned。然而，当您以这种方式偏离理论时，您必须采取相应的(并且有些不明显的)步骤，即将 IKF 状态向量的校正值归零。换句话说，在你执行 Pned = Pned + dx(1:3) 之后，你已经“使用”了校正，你需要用 dx(1:3) = dx(1:3) - dx(1:3)(简化:dx(1:3) = 0)，这样您就不会不经意地随着时间的推移整合校正。

为什么会这样？为什么它不会弄乱过滤器的其余部分？事实证明，KF 过程协方差 P 实际上并不依赖于状态 x。这取决于更新功能和过程噪声 Q 等。所以过滤器不关心数据是什么。 (现在这是一个简化，因为通常 Q 和 R 包含旋转项，并且 R 可能会根据其他状态变量等而变化，但在在那些情况下，您实际上使用的是来自过滤器外部的状态(累积位置和方向)，而不是原始校正值，它们本身没有任何意义。