3

我正在尝试使用Indirect Kalman Filter实现惯性导航系统。我发现了很多关于这个主题的出版物和论文，但没有太多的代码作为例子。对于我的实施，我正在使用以下链接中提供的硕士论文：

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。

谢谢。

4

1 回答 1

3

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

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

理论上，当您使用间接形式时，您可以自由集成 IMU（该过程在该论文中称为机械化）并偶尔运行 IKF 以更新其校正。理论上，加速度计未经检查的双重积分会在和中产生很大的误差值（或者对于廉价的 MEMS IMU，会产生巨大的误差值）。这反过来又会导致 IKF随着时间的推移产生相应的大值，而未经检查的 IMU 集成离真相越来越远。从理论上讲，您可以随时对您的位置进行采样（符号并不重要——您可以通过任何一种方式进行设置）。这里重要的部分是你没有修改PnedVneddx(1:6)Pned +/- dx(1:3)Pned来自 IKF，因为两者都是相互独立运行的，当您需要答案时，您可以将它们加在一起。

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

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

于 2014-11-08T22:58:57.510 回答