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