扩展卡尔曼滤波定位步骤使用不确定性椭圆表示

《摘自概率机器人》 上接马尔可夫定位

1. 预测步骤

\qquad ${\mathbf{\Sigma }}_{t-1},{\mathbf{\mu }}_{t-1}$分别表示$t-1$时刻的位置方差和期望，$${\overline{\mathbf{\Sigma }}}_t={\mathbf{G}}_t{\mathbf{\Sigma }}_{t-1}{\mathbf{G}}_t^T+{\mathbf{V}}_t{\mathbf{M}}_t{\mathbf{V}}_t^T$$ \qquad 等式右端第一项$t-1$时刻的位置方差到$t$时刻的转换，第二项表示由控制噪声从控制空间映射到状态空间产生的运动噪声，所以第二项受到位移(线速度)和旋转(角速度) 噪声的影响。上图中$b),c),d)$分别表示高平移噪声、高旋转噪声和高平移、旋转噪声。

2. 测量预测

\qquad 该步骤为使用预测机器人位置和测量不确定性表示测量预测，预测位置和不确定性使用下图表示：

\qquad 预测不确定性使用${\mathbf{S}}_t$，由两个高斯函数卷积生成。
$${\mathbf{S}}_t={\mathbf{H}}_t{\overline{\mathbf{\Sigma }}}_{\mathbf{t}}{\mathbf{H}}_{\mathbf{t}}^{\mathbf{T}}+{\mathbf{Q}}_t$$ \qquad 上式第一项为由位置不确定性带来的不确定性，第二项为固有的测量噪声，$z_t$为实际测量值，${\overline{z}}_t$为预测测量值。更新向量为$z_t-{\overline{z}}_t$更新向量越短，测量值越可靠。

3. 估计更新

\qquad 计算卡尔曼滤波增益$K_t$，按比例决定更新向量。更新算法如下：

\qquad 更新不确定椭圆

示例，如图所示机器人按照运动控制轨迹(虚线)和实际运动轨迹(实线)