扩展卡尔曼滤波定位（extended Kalman filter localization， EKF localization）是之前说的马尔可夫定位中的一种特殊情况。在EKF定位算法中，我们假设地图是由一系列特征组成（a collection of features），并且每个特征都是独特的（uniquely identifiable）。在t时刻的任何一个点，机器人可以通过传感器获得由与附近特征的距离、方向组成的向量：
每个测量值z对应一个实际的环境特征c。
一. EKF定位举例：
为了使EKF中的belief值是单峰形式（unimodal shape），做两个假设：
1、我们假设对应关系是已知的，比如上面有三个门，每个门是一个特征，对应一个label（1,2,3），测量模型是p(zt | xt, m, ct)，其中m是地图，ct ∈ {1,2,3}。
2、我们假设初始位置是大概知道的，典型的初始化位置是知道一个范围，belief值用高斯分布表示，图a所示；随着机器人的向右移动，belief值会卷积上运动模型（motion model），获得一个平移的并且变宽的高斯分布，图b所示。
接下来，机器人检测到自己在第二个门（ct = 2）前面，在图c的第一个坐标上表示的就是这次观测p(zt | xt, m, ct)，同样也是高斯型，与上面求得的belief值相乘，获得后验概率，如图c第二个坐标系上显示。可以看到，计算获得的belief值的方差比之前的belief预测值和测量值都小，高斯曲线更尖陡，这是必须的，因为有两个独立的估计（independent estimates）可以让我们获得更加确信的belief值。
最后，机器人继续向右移动，由于机器人的运动增加了belief值的不确定性，使得高斯曲线又变宽了，图d所示。
二. EKF定位算法：
1、输入是t-1时刻高斯估计的机器人位姿，用均值（mean） μt−1 和协方差（covariance） Σt−1表示；控制量ut；地图m；t时刻的测量值zt和对应的特征变量ct：
2、输出是更新后的预测机器人位姿（μt，Σt）和特征观测的似然（likelihood）概率pzt ；
3、第3、4行计算雅可比矩阵（Jacobians），供线性化后的运动模型（linearized motion model）使用；
4、第5行是运动噪声的协方差矩阵；
5、第6、7行是运动后的更新（motion update），位姿估计μt 在第6行求出，对应的不确定椭圆（uncertainty ellipse）在第7行求出；
6、从第8到21行都是测量更新（measurement update）阶段，此阶段的核心是遍历t时刻所有观测到的特征 i；
7、第10行把测量向量中对应的第i个特征赋值给j；
8、第12行求得预测的测量值z；
9、第13行求得测量模型的雅可比矩阵H；
10、第14行求得对应着预测的测量值z（第12行求的结果）的不确定度（uncertainty）；
11、第15行求出卡尔曼增益（Kalman gain）K；
12、第16、17行通过测量值又更新了位姿估计μt 和不确定椭圆Σt；
13、第19、20行最后更新了位姿估计μt 和不确定椭圆Σt；
14、第21行求得测量似然概率pzt 。
长按二维码，关注作者个人微信公众号
长按二维码，关注灵喵机器人微信公众号
长按二维码，关注灵喵机器人微信公众号
