1.SLAM问题
1.1建模
考虑某个状态 \(\xi\),以及一次与该变量相关的观测 \(r_{i}\)。由于噪声存在,观测服从概率分布 \(p(r_{i}|\xi)\)。多个观测时,各个测量值相互独立,则多个测量 \(r = (r_{1}, ..., r_{n})^{T}\)构成的似然概率为:
![](https://img-blog.csdnimg.cn/img_convert/d0020a9e0d206743b1af046370f6e44b.png)
如果知道机器人状态的先验信息 \(p(\xi)\),如GPS,码盘信息等,则由贝叶斯法则,有后验概率:
![](https://img-blog.csdnimg.cn/img_convert/c968d57dab158f66286d578653fbb9f1.png)
通过最大后验估计,获得系统状态的最优估计:
![](https://img-blog.csdnimg.cn/img_convert/cc7ac65e998f36fd913018ef4f3fbe51.png)
意为在当前的观测下,什么样的系统状态出现的概率最大。
分母跟状态量无关,舍弃取log。即:
![](https://img-blog.csdnimg.cn/img_convert/67c829d579c8f920d4caa75011146cc2.png)
如果观测服从多元高斯分布:
![](https://img-blog.csdnimg.cn/img_convert/ffee86c4b5743cb2b4269676344a02aa.png)
则有:
![](https://img-blog.csdnimg.cn/img_convert/3ef60fae74e7583e4b7e9081183d5f5f.png)
该问题可由最小二乘求解:
![](https://img-blog.csdnimg.cn/img_convert/0c7869a168f2cc437981c5027e5dc8b4.png)
零均值的多元高斯分布概率为:\(p(x) = \frac{1}{Z}exp(-\frac{1}{2}x^{T}\Sigma^{-1}x)\)
其中\(\Sigma\)是协方差矩阵。\(\Sigma^{-1}\)为协方差矩阵的逆,即信息矩阵。
2.边缘化信息矩阵
使用滑动窗口不可避免的要移除掉变量,当移除变量的时候,之前与之不相关的变量,有可能变得相关,所以要对信息矩阵进行维护。
这里使用舒尔补来对信息矩阵进行边缘化处理。
2.1舒尔补的定义:
给定任意的矩阵块M,如下所示:
![](https://img-blog.csdnimg.cn/img_convert/b647e4be2745aae9312c12a2eba7a5fa.png)
如果,矩阵块D是可逆的,则 \(A-BD^{-1}C\)称之为D关于M的舒尔补
如果。矩阵块A是可你的,则 \(D-CA^{-1}B\)称之为A关于M的舒尔补
2.2使用舒尔补来去除变量:
假如一个系统三个变量x1,x2,x3。其信息矩阵为:
![](https://img-blog.csdnimg.cn/img_convert/bffdd5ccac6e13ad5cb701397f7f44cf.png)
那么把x3去掉,也就是把蓝色部分去掉,利用舒尔补。
![](https://img-blog.csdnimg.cn/img_convert/43b6ec82f35175baada6eb8fe095f313.png)
则去掉后的信息矩阵为:
![](https://img-blog.csdnimg.cn/img_convert/b53d6df55feb4fdfa3cc792d69978bd5.png)
3.滑动窗口算法
![](https://img-blog.csdnimg.cn/img_convert/ec91282020c826e1c6bfeff93ae773ce.png)
上述最小二乘问题,对应的高斯牛顿求解为:
![](https://img-blog.csdnimg.cn/img_convert/6377b81a5e990c51ad6a4c084dd4586a.png)
公式中的雅可比矩阵为:
![](https://img-blog.csdnimg.cn/img_convert/9ca22516ca0e65325ea294cc6e16f6bf.png)
矩阵乘法公式可以写成连加:
![](https://img-blog.csdnimg.cn/img_convert/88dd990b7d7ece5de366140af4ede441.png)
![](https://img-blog.csdnimg.cn/img_convert/f5a130a494f19b24504616ff90fb0178.png)
![](https://img-blog.csdnimg.cn/img_convert/50bab03b7e325834d73ff46377396917.png)
使用边际概率移除变量\(\xi_{1}\),信息矩阵的变化过程:
![](https://img-blog.csdnimg.cn/img_convert/ed6bf156bd027902239d691606ef7085.png)