1. SLAM问题定义
同时定位与建图（SLAM）的本质是一个估计问题，它要求移动机器人利用传感器信息实时地对外界环境结构进行估计，并且估算出自己在这个环境中的位置，Smith 和Cheeseman在上个世纪首次将EKF估计方法应用到SLAM。
以滤波为主的SLAM模型主要包括三个方程：
1）运动方程：它会增加机器人定位的不确定性
2）根据观测对路标初始化的方程：它根据观测信息，对新的状态量初始化。
3）路标状态对观测的投影方程：根据观测信息，对状态更新，纠正，减小不确定度。
2. EKF-SLAM维护的数据地图
系统状态x是一个很大的向量，它包括机器人的状态和路标点的状态，
[ mathbf{x}=left[egin{array}{c}
{mathcal{R}} \
{mathcal{M}}
end{array}
ight]=left[egin{array}{c}
{mathcal{R}} \
{mathcal{L}_{1}} \
{vdots} \
{mathcal{L}_{n}}
end{array}
ight]
]
其中({mathcal{R}})是机器人状态，({mathcal{M}} = left({mathcal{L}_{1}}， dots，{mathcal{L}_{n}}
ight))是n个当前已经观测过的路标点状态集合。
在EKF中，x被认为服从高斯分布，所以，EKF-SLAM的地图被建模为x的均值(overline{x})与协方差(mathbf{P}),
[ overline{mathbf{x}}=left[frac{overline{mathcal{R}}}{mathcal{M}}
ight]=left[egin{array}{c}
{overline{mathcal{R}}} \
{overline{mathcal{L}}_{1}} \
{vdots} \
{overline{mathcal{L}}_{n}}
end{array}
ight] quad mathbf{P}=left[egin{array}{cc}
{mathbf{P}_{mathcal{R} mathcal{R}}} & {mathbf{P}_{mathcal{R} mathcal{M}}} \
{mathbf{P}_{mathcal{M R}}} & {mathbf{P}_{mathcal{M M}}}
end{array}
ight]=left[egin{array}{cccc}
{mathbf{P}_{mathcal{R R}}} & {mathbf{P}_{mathcal{R} mathcal{L}_{1}}} & {cdots} & {mathbf{P}_{mathcal{R} mathcal{L}_{n}}} \
{mathbf{P}_{mathcal{L}_{1} mathcal{R}}} & {mathbf{P}_{mathcal{L}_{1} mathcal{L}_{1}}} & {cdots} & {mathbf{P}_{mathcal{L}_{n}} mathcal{L}_{n}} \
{vdots} & {vdots} & {ddots} & {vdots} \
{mathbf{P}_{mathcal{L}_{n} mathcal{R}}} & {mathbf{P}_{mathcal{L}_{n} mathcal{L}_{1}}} & {cdots} & {mathbf{P}_{mathcal{L}_{n} mathcal{L}_{n}}}
end{array}
ight]
]
因此EKF-SLAM的目标就是根据运动模型和观测模型及时更新地图量(left{overline{mathbf{x}},mathbf{P}
ight})
3. EKF-SLAM算法实施
3.1 地图初始化
显而易见，在机器人开始之前是没有任何路标点的信息的，因此此时地图中只有机器人自己的状态信息，因此({n} = 0,{x} = {mathcal{R}})。SLAM中经常把机器人初始位姿认为是地图的原点，其初始协方差可以按实际情况设定，比如：
[ overline{mathbf{x}}=left[egin{array}{l}
{x} \
{y} \
{ heta}
end{array}
ight]=left[egin{array}{l}
{0} \
{0} \
{0}
end{array}
ight] quad mathbf{P}=left[egin{array}{lll}
{0} & {0} & {0} \
{0} & {0} & {0} \
{0} & {0} & {0}
end{array}
ight]
]
3.2 运动模型
在EKF中如果x是状态量，u是控制输入，n是噪声变量，那么我们可以得到一般的状态更新函数：
[mathbf{x} leftarrow f(mathbf{x}, mathbf{u}, mathbf{n})
]
EKF的预测步骤为：
[egin{array}{l}
{overline{mathbf{x}} leftarrow f(overline{mathbf{x}}, mathbf{u}, 0)} \
{mathbf{P} leftarrow mathbf{F}_{mathbf{x}} mathbf{P} mathbf{F}_{mathbf{x}}^{ op}+mathbf{F}_{mathbf{n}} mathbf{N} mathbf{F}_{mathbf{n}}^{ op}}
end{array}
]
其中雅克比矩阵(mathbf{F}_{mathbf{x}}=frac{partial f(overline{mathbf{x}}, mathbf{u})}{partial mathbf{x}})，(mathbf{F}_{mathbf{n}}=frac{partial f(overline{mathbf{x}}, mathbf{u})}{partial mathbf{n}})，({mathbf{N}})是随机变量n的协方差。
但是在EKF-SLAM中，只有一部分状态({mathcal{R}})是随运动而改变的，其余所有路标状态不改变，所以SLAM的运动方程为：
[egin{array}{l}
{mathcal{R} leftarrow f_{mathcal{R}}(mathcal{R}, mathbf{u}, mathbf{n})} \
{mathcal{M} leftarrow mathcal{M}}
end{array}
]
因此我们可以得到稀疏的雅克比矩阵：
[ mathbf{F}_{mathbf{x}}=left[egin{array}{cc}
{frac{partial f_{mathcal{R}}}{partial mathcal{R}}} & {0} \
{0} & {mathbf{I}}
end{array}
ight] quad mathbf{F}_{mathbf{n}}=left[egin{array}{c}
{frac{partial f_{mathcal{R}}}{partial mathbf{n}}} \
{0}
end{array}
ight]
]
最终我们得到了用于运动模型的EKF稀疏预测公式
[overline{mathcal{R}} leftarrow f_{mathcal{R}}(overline{mathcal{R}}, mathbf{u}, 0)
]
[ mathbf{P}_{mathcal{R} mathcal{R}} leftarrow frac{partial f_{mathcal{R}}}{partial mathcal{R}} mathbf{P}_{mathcal{R} mathcal{R}} frac{partial f_{mathcal{R}}^{T}}{partial mathcal{R}}+frac{partial f_{mathcal{R}}}{partial mathbf{n}} mathbf{N} frac{partial f_{mathcal{R}}^{T}}{partial mathbf{n}}
]
[mathbf{P}_{mathcal{R} mathcal{M}} leftarrow frac{partial f_{mathcal{R}}}{partial mathcal{R}} mathbf{P}_{mathcal{R} mathcal{M}}
]
[ mathbf{P}_{mathcal{M} mathcal{R}} leftarrow mathbf{P}_{mathcal{R} mathcal{M}}^{ op}
]
3.3 已经加入地图的状态量观测更新
在EKF中，我们有以下一般的观测方程
[ mathbf{y}=h(mathbf{x})+mathbf{v}
]
其中(mathbf{y})是测量噪声，(mathbf{x})是全状态，(h())是观测函数，(v)是测量噪声。
典型的EKF观测更新为：
[ overline{mathbf{z}}=mathbf{y}-h(overline{mathbf{x}})
]
[mathbf{Z}=mathbf{H}_{mathbf{x}} mathbf{P} mathbf{H}_{mathbf{x}}^{ op}+mathbf{R}
]
[ mathbf{K}=mathbf{P} mathbf{H}_{mathbf{x}}^{ op} mathbf{Z}^{-1}
]
[overline{mathbf{x}} leftarrow overline{mathbf{x}}+mathbf{K} overline{mathbf{z}}
]
[ mathbf{P} leftarrow mathbf{P}-mathbf{K} mathbf{Z} mathbf{K}^{ op}
]
雅克比矩阵(mathbf{H}_{mathbf{x}}=frac{partial h(overline{mathbf{x}})}{partial mathbf{x}})，(mathbf{R})是测量噪声的协方差矩阵。
在SLAM中，观测指的是机器人上的传感器观测到某些路标点，并对路标点进行参数化的输出。每次可能对路标点有多个观测值，这里每使用一个观测值，就进行一次状态更新。
观测的结果依赖于机器人的状态(mathcal{R}),传感器的状态(mathcal{S})和路标点的状态(mathcal{L}_{i})，并且这里假设，传感器的状态与机器人的状态差了一个固定的坐标变化，其实也就是外参固定。当观测到路标点(i)时，可以得到如下关系：
[ mathbf{y}_{i}=h_{i}left(mathcal{R}, mathcal{S}, mathcal{L}_{i}
ight)+mathbf{v}
]
这就是观测方程，它不依赖于除了(mathcal{L}_{i})外的任何路标点状态。因此EKF-SLAM的雅克比(mathbf{H}_{mathbf{x}})也是稀疏的：
[ mathbf{H}_{mathbf{x}}=left[egin{array}{lllllllll}
{mathbf{H}_{mathcal{R}}} & {0} & {cdots} & {0} & {mathbf{H}_{mathcal{L}_{i}}} & {0} & {cdots} & {0}
end{array}
ight]
]
其中(mathbf{H}_{mathcal{R}}=frac{partial h_{i}left(overline{mathcal{R}}, mathcal{S}, overline{mathcal{L}}_{i}
ight)}{partial mathcal{R}})，(mathbf{H}_{mathcal{L}_{i}}=frac{partial h_{i}left(overline{mathcal{R}}, mathcal{S}, overline{mathcal{L}}_{i}
ight)}{partial mathcal{L}_{i}})。由于这里的稀疏性，EKF-SLAM的观测更新变成：
[overline{mathbf{z}}=mathbf{y}_{i}-h_{i}left(overline{mathcal{R}}, mathcal{S}, overline{mathcal{L}}_{i}
ight)
]
[ mathbf{Z}=left[egin{matrix}mathbf{H}_{mathcal{R}} &mathbf{H}_{mathcal{L}_{i}}end{matrix}
ight]left[egin{array}{cc}
{mathbf{P}_{mathcal{R} mathcal{R}}} & {mathbf{P}_{mathcal{R} mathcal{L}_{i}}} \
{mathbf{P}_{mathcal{L}_{i} mathcal{R}}} & {mathbf{P}_{mathcal{L}_{i} mathcal{L}_{i}}}
end{array}
ight]left[egin{array}{c}
{mathbf{H}_{mathcal{R}}^{ op}} \
{mathbf{H}_{mathcal{L}_{i}}^{ op}}
end{array}
ight]+mathbf{R}
]
[mathbf{K}=left[egin{array}{ll}
{mathbf{P}_{mathcal{R} mathcal{R}}} & {mathbf{P}_{mathcal{R} mathcal{L}_{i}}} \
{mathbf{P}_{mathcal{M} mathcal{R}}} & {mathbf{P}_{mathcal{M} mathcal{L}_{i}}}
end{array}
ight]left[egin{array}{l}
{mathbf{H}_{mathcal{R}}^{ op}} \
{mathbf{H}_{mathcal{L}_{i}}^{ op}}
end{array}
ight] mathbf{Z}^{-1}
]
[ overline{mathbf{x}} leftarrow overline{mathbf{x}}+mathbf{K} overline{mathbf{z}}
]
[mathbf{P} leftarrow mathbf{P}-mathbf{K} mathbf{Z} mathbf{K}^{ op}
]
3.4 观测方程可逆时的状态增广
这里的状态增广指的是新发现的路标点的初始化。当机器人发现了曾经未观测到的路标点时，会利用观测方程将新的路标状态加入地图，这一步操作会增大总状态向量的大小。可以看到EKF-SLAM中的滤波器大小动态变化的。
当传感器信息可以提供新发现路标点的所有自由度，也就是观测方程是双射时，只需要根据观测方程(h())的逆运算，即可以得到机器人状态(mathcal{R})，传感器状态(mathcal{S})，观测量(mathbf{y}_{n+1})，观测噪声(v)，它们与新路标点状态的关系：
[ mathcal{L}_{n+1}=gleft(mathcal{R}, mathcal{S}, mathbf{y}_{n+1},v
ight)
]
上式是单个路标点的逆观测模型。
路标点的均值和雅克比：
[ overline{mathcal{L}}_{n+1}=gleft(overline{mathcal{R}}, mathcal{S}, mathbf{y}_{n+1},0
ight)
]
[mathbf{G}_{mathcal{R}}=frac{partial gleft(overline{mathcal{R}}, mathcal{S}, mathbf{y}_{n+1},0
ight)}{partial mathcal{R}}
]
[ mathbf{G}_{mathbf v}=frac{partial gleft(overline{mathcal{R}}, mathcal{S}, mathbf{y}_{n+1},0
ight)}{partial mathbf{v}}
]
显然，新加路标点状态的协方差(mathbf{P}_{mathcal{L} mathcal{L}})，以及该状态与地图其它状态的互协方差为：
[ mathbf{P}_{mathcal{L} mathcal{L}}=mathbf{G}_{mathcal{R}} mathbf{P}_{mathcal{R} mathcal{R}} mathbf{G}_{mathcal{R}}^{ op}+mathbf{G}_{mathbf v} mathbf{R} mathbf{G}_{mathbf v}^{ op}
]
[mathbf{P}_{mathcal{L} mathbf{x}}=mathbf{G}_{mathcal{R}} mathbf{P}_{mathcal{R} mathbf{x}}=mathbf{G}_{mathcal{R}}left[egin{matrix}mathbf{P}_{mathcal{R} mathcal{R}} &mathbf{P}_{mathcal{R} mathcal{M}}end{matrix}
ight]
]
然后将这些结果加入到地图中，可以得到总状态的均值与协方差矩阵：
[ overline{mathbf{x}} leftarrow
left[egin{array}{c}
{overline{mathbf{x}}} \
{overline{mathcal{L}}_{n+1}}
end{array}
ight]
]
[ mathbf{P} leftarrowleft[egin{array}{cc}
{mathbf{P}} & {mathbf{P}_{mathcal{L} mathbf{x}}^{ op}} \
{mathbf{P}_{mathcal{L}mathbf{x}}} & {mathbf{P}_{mathcal{L}mathcal{L}}}
end{array}
ight]
]
4. 仿真实验
4.1 模型设置
4.1.1 传感器模型
传感器是一个360度的雷达，可以探测发现周围一定范围内的路标点及其类型，其探测半径为r，其观测值为((xi)，s)。(xi)为在当前雷达坐标系中路标点与x轴的的夹角，s为路标点与当前雷达坐标系原点的距离。
4.1.2 运动模型
将当前时刻雷达局部坐标系中的（({u}_{1}),0）点作为下一时刻雷达的位置，所以运动方程可以设为：
[ left[egin{array}{cc}
{x_{n}} \
{y_{n}}
end{array}
ight] =
left[egin{array}{cc}
cos heta_{n-1} & -sin heta_{n-1} \
sin heta_{n-1} & cos heta_{n-1}
end{array}
ight]
left[egin{array}{cc}
{u}_{1} \
0
end{array}
ight] +
left[egin{array}{cc}
{x_{n-1}} \
{y_{n-1}}
end{array}
ight] +
left[egin{array}{cc}
{n}_{1} \
0
end{array}
ight]
]
其方位每次增加固定的(u_2):
[ { heta_{n+1}} = { heta_n} + {u}_{2} + {n}_{2}
]
其中(n_1,n_2)为系统噪声。
4.1.3 观测模型
设路标点(i)的状态为(x_{L_i}=)((m_1)，({m}_{2}))，则其在当前雷达坐标系的坐标为：
[ left[egin{array}{cc}
{{{ladar}_{1}}} \
{{{ladar}_{2}}}
end{array}
ight] =
{left[egin{array}{cc}
cos heta_n & -sin heta_n \
sin heta_n & cos heta_n
end{array}
ight]}^{-1}
left[egin{array}{cc}
{{{m}_{1}}} \
{{{m}_{2}}}
end{array}
ight] -
left[egin{array}{cc}
{x_n} \
{y_n}
end{array}
ight]
]
则观测量为：
[ xi = atan2left({{{ladar}_{2}}},{{{ladar}_{1}}}
ight) + {v}_{1}
]
[ s = sqrt{{left({{{ladar}_{1}}}
ight)}^{2} + {left({{{ladar}_{2}}}
ight)}^{2}} + {v}_{2}
]
其中(v_1,v_2)为测量噪声。
4.2 实验结果
使用MATLAB编写程序进行仿真。
代码地址:https://github.com/liuzhenboo/EKF-2D-SLAM
4.2.1 第一次状态增广
其中左图黑色的点表示滤波器新加入的路标点状态均值，绿色椭圆表征路标状态的协方差，其椭圆方程为：
[ (mathbf{x}-overline{mathbf{x}})^{ op} mathbf{P}^{-1}(mathbf{x}-overline{mathbf{x}})= ext { const }
]
其中x为路标状态, P为路标的协方差。
程序中是对P进行SVD分解，得到椭圆的方向R以及半轴到的长度，进行绘图。
4.2.2 状态增广，观测更新
如上图，黑色椭圆对应的路标点表示雷达曾经观测过它，但是当前时刻没有观测到；红色椭圆对应的路标点表示雷达曾经观测过它，并且当前时刻也观测到了；蓝色代表当前刚刚初始化的新路标点。
注意：由于数值计算的原因，图中几何元素的位置关系可能和实际有些许差别；比如有的路标点明明不在雷达范围内，却开始初始化（绿色），这是因为计算过程中matlab四舍五入导致的结果。
4.2.3 状态不再增广，只有观测更新
如上图，不再存在蓝色的协方差椭圆，代表状态增广停止，滤波器的大小不再改变。