我正在尝试实现用于方向检测的卡尔曼滤波器。就像我在网上找到的大多数其他实现一样，我将使用陀螺仪和加速度计来测量俯仰和滚动，但是我还打算添加水平检测。这将使我对俯仰和滚动进行第二次阅读。这意味着我将有两种测量当前状态的方法，加速度计和水平检测，而陀螺仪将用于控制。
到目前为止，我已经根据本教程分别实现了传感器数据和水平检测的过滤器：http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/
我必须修改卡尔曼滤波器的哪一部分，以便算法在预测状态、加速度计读数和水平检测读数之间选择最佳读数？任何帮助，论文或网站的链接将不胜感激 提前感谢您的帮助
【问题讨论】：
标签： orientation kalman-filter sensor-fusion inertial-navigation
KF 由两个并行组件组成： 1. 估计状态，并且 2. 该估计的不确定性（特别是状态分量的协方差矩阵）。
当组合状态的 2 个估计值时，标准方法采用加权平均值，权重是（协）方差的倒数。也就是说，2 个估计值中越确定（协方差越小），其权重越高。
因此，如果您尚未跟踪 2 个估计值的协方差，则需要执行此操作。
对于具有 2 个估计值 X' 和 X" 的标量状态 X，每个估计值都有一个方差：V' 和 V"，其倒数 C' = 1/V' 和 C" = 1/V"。 （“确定性”C 比方差 V 更易于使用。）
然后状态的 MMSE 估计（这是 KF 尝试优化的）由下式给出：Xmmse = (X'/V' + X"/V") / (1/V' + 1/V" ）。 [此时对V也有相应的更新，基于V'和V"。]
对于矢量状态，V 将被一个协方差矩阵替换，并且除法将变为矩阵逆。在这种情况下，直接跟踪逆 C 可能更容易： Xmmse = (C' + C") \ (C' * X' + C" * X") [以及对 C 的相应更新]， 其中“\”表示预乘第一个因子的倒数，C'+C”。
我希望这会有所帮助。
[我为这里糟糕的格式道歉。 Stack Overflow 推断代数表达式是代码，并要求在让我回答之前将它们格式化为代码。他们不是，所以我不能。]
【讨论】：