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值，因为你会失去精确度（因为需要很多有效位来代表巨大的部分）而不是你想要的精度）。您已经意识到，在 practice 中，您希望在每次更新时递归更新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可能因其他状态变量等而异，但在这些情况下，您实际上是在使用外部状态过滤器（累积位置和方向）不是原始校正值，它们本身没有意义。）

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

1 个答案: