orientation - 具有冗余状态测量的卡尔曼滤波器

Question

我正在尝试实现用于方向检测的卡尔曼滤波器。就像我在网上找到的大多数其他实现一样，我将使用陀螺仪和加速度计来测量俯仰和滚动，但我还打算添加水平检测。这将使我对俯仰和滚动进行第二次阅读。这意味着我将有两种测量当前状态的方法，加速度计和水平检测，而陀螺仪将用于控制。

到目前为止，我已经根据本教程分别实现了传感器数据和水平检测的过滤器:http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/

我必须修改卡尔曼滤波器的哪一部分，以便算法在预测状态、加速度计读数和水平检测读数之间选择最佳读数？任何帮助，论文或网站的链接将不胜感激提前感谢您的帮助

Answer 1

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 推断代数表达式是代码，并要求在让我回答之前将它们格式化为代码。他们不是，所以我不能。]