我正在尝试实现用于方向检测的卡尔曼滤波器。就像我在网上找到的大多数其他实现一样，我将使用陀螺仪和加速度计来测量俯仰和滚动，但我还打算添加水平检测。这将使我对俯仰和滚动进行第二次阅读。这意味着我将有两种测量当前状态的方法，加速度计和水平检测，而陀螺仪将用于控制。
到目前为止，我已经根据本教程分别实现了传感器数据和水平检测的过滤器：http: //blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-实现它/
我必须修改卡尔曼滤波器的哪一部分，以便算法在预测状态、加速度计读数和水平检测读数之间选择最佳读数？任何帮助、论文或网站的链接将不胜感激提前感谢您的帮助
KF 由两个平行分量组成：1. 估计状态，以及 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 推断代数表达式是代码，并要求在让我回答之前将它们格式化为代码。他们不是，所以我不能。]