这个一张经典的EKF定位示例图:
如上图所示,小萝卜头走向1,2,3号门时,下方的置信度都会有所不同。给出2个假设: 1)每个门对应一个label(1,2,3),测量模型是p(z_t|x_t, m, c_t),其中m是地图,c_t∈{1,2,3}。 2)假设初始位置已知,置信度值用高斯分布表示,如图a所示;随着机器人向右运动,置信度值会卷积上运动模型,获得一个平移且变宽的高斯分布,如图b所示 接下来,机器人检测到自己在第二个门(c_t = 2)前面,在图c的第一个坐标上表示的就是这次观测 p(z_t|x_t, m, c_t),同样也是高斯型,与上面求得的置信度值相乘,获得后验概率,如图c第二个坐标系上显示。可以看到,计算获得的置信度值的方差比之前的置信度预测值和测量值都小,高斯曲线更尖陡,这是必须的,因为有两个独立的估计(independent estimates)可以让我们获得更加确信的置信度值。 最后,机器人继续向右移动,由于机器人的运动增加了置信度值的不确定性,使得高斯曲线又变宽了,图d所示。接着,给出EKF算法思路:
如上图所示,这是一个基于扩展卡尔曼滤波的(EKF)的传感器融合定位示意图。蓝线是真实轨迹,黑线是航位推测轨迹,绿点是定位观测(例如GPS),红线是用EKF估算的轨迹,红色椭圆是用EKF估算的协方差椭圆。1.滤波器设计
在仿真中,机器人的状态向量在时刻t处包含4个状态:X_t=[x_t,y_t,ϕ_t,v_t] 其中,x,y 是二维坐标系下的位置,ϕ是朝向,v是速度。 以代码为例: 1)xEst:状态向量 2)P_t: 状态的协方差矩阵 3)Q: 处理噪声时的协方差矩阵 4)R: 时刻t观测噪声的协方差矩阵 机器人有一个速度传感器和陀螺仪传感器,所以输入向量能在每个时刻被用作: u_t = [v_t, w_t] 此外,机器人还有一个GNSS传感器,这意味着机器人可以观察每个时刻的x、y坐标。 z_t = [x_t, y_t] 输入向量和观测向量都包含有传感器噪声。2.运动模型
机器人模型是 所以,运动模型为: X_t+1 = Fx_t + Bu_t 其中, d_t 是时间间隔。 参看如下代码: 雅克比矩阵为:3.观测模型
机器人可以从GPS中获得$xy$位置信息,所以GPS观测模型为:z_t = Hx_t 其中, 它的雅克比矩阵为:4.扩展卡尔曼滤波
———————————预测————————-x_pred = Fx_t + Bu_t
P_pred = J_FP_tJ^T_F+Q
———————————更新————————Z_pred = Hx_pred
y = z- z_pred
S= J_HP_pred.J^T_H + R
K = P_Pred.J^T_HS^-1
x_t+1 = x~pred + Ky
P_t+1 = (I - KJ_H)P~pred
评论(0)
您还未登录,请登录后发表或查看评论