我正在尝试使用间接卡尔曼滤波器实现惯性导航系统。我找到了很多关于这个主题的出版物和论文，但没有太多的代码作为示例。对于我的实现，我使用硕士论文可在以下链接:
https://fenix.tecnico.ulisboa.pt/downloadFile/395137332405/dissertacao.pdf如第47页所述，惯性传感器的测量值等于真实值加上一系列其他项(偏差，比例因子，…)。对于我的问题，让我们只考虑偏见。
:
Wmeas = Wtrue + BiasW (Gyro meas)
Ameas = Atrue + BiasA. (Accelerometer meas)
我应该使用"true"值，或者更好:
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.
谢谢。
你遇到的困难是理论和实践之间的差异。从代码片段中取出代码，而不是问题中的符号版本:
% 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可能根据其他状态变量等而变化，但在这些情况下，您实际上使用的是来自过滤器外部的状态(累积位置和方向)，而不是原始校正值，它们本身没有意义)。