扩展卡尔曼滤镜(EKF)是机器人本地化中广泛使用的算法,它通过将传感器数据与机器人运动的数学模型相结合来估计机器人的位置和方向,了解EKF的数学基础对于有效的执行和调试至关重要.

国家代表制和预测

EKF的核心涉及将机器人状态作为矢量来表示,一般包括位置和方向. 预测步骤使用非线性运动模型来预测当前状态的及时性,这涉及计算运动模型的雅各比矩阵,将非线性方程线性化围绕当前估计.

测量更新和线性化

传感器测量是通过更新步骤结合的。由于测量往往是状态的非线性函数,所以EKF使用他们的雅各比人来将这些函数线性化。这个过程根据预期传感器读数和实际传感器读数的差值来调整预测状态。

数学方程式

预测方程是:

国家预测:]

x ⁇ k+1−=f(x ⁇ k, uk) ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇

共变预测:]

Pk+1−=Fk Pk Fkt+Qk 双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双双

其中f是非线性运动模型,Fk是它的雅各宾语,P是共变矩阵,Q是过程噪声共变.

更新方程式如下:

卡尔曼增益:

Kk = Pk = Hkt (Hk Pk = Hkt + Rk)− 1 个

国家最新情况:]

x ⁇ k = x ⁇ k – + Kk(zk - h(x ⁇ k -))) ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇

共变更新:]

Pk= (一-Kk Hk) ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇ ⁇

这里h是非线性测量模型,Hk是它的雅各比亚语,Rk是测量噪声共变,zk是实际的传感器测量.