舒尔补在SLAM中的应用_舒尔补是用来干嘛的-程序员宅基地
舒尔补在SLAM中的应用
1.舒尔补的定义
对于任意的矩阵 M M M，如下所示
(1) M = [ A B C D ] M = \left[ \begin{matrix} A& B\\ C& D \\ \end{matrix}\right]\tag{1} M=[ACBD](1)
如果矩阵块 D D D是可逆的，则 A − B D − 1 C A-BD^{-1}C A−BD−1C 称之为 D D D 关于 M M M的舒尔补。
如果矩阵块 A A A是可逆的，则 D − C A − 1 B D-CA^{-1}B D−CA−1B 称之为 A A A 关于 M M M的舒尔补。
2.舒尔补的由来
在将 M M M变为上三角和下三角的过程中，都会遇到舒尔补：
(2) [ I 0 − C A − 1 I ] [ A B C D ] = [ A B 0 Δ A ] \left[ \begin{matrix} I& 0\\ -CA^{-1}& I \\ \end{matrix}\right] \left[ \begin{matrix} A& B\\ C& D \\ \end{matrix}\right] = \left[ \begin{matrix} A& B\\ 0& \Delta A \\ \end{matrix}\right] \tag{2} [I−CA−10I][ACBD]=[A0BΔA](2)
(3) [ A B C D ] [ I − A − 1 B 0 I ] = [ A 0 C Δ A ] \left[ \begin{matrix} A& B\\ C& D \\ \end{matrix}\right] \left[ \begin{matrix} I& -A^{-1}B\\ 0& I \\ \end{matrix}\right] = \left[ \begin{matrix} A& 0\\ C& \Delta A \\ \end{matrix}\right] \tag{3} [ACBD][I0−A−1BI]=[AC0ΔA](3)
其中： Δ A = D − C A − 1 B \Delta A =D-CA^{-1}B ΔA=D−CA−1B。将两式联合起来，将M变形为对角形：
(4) [ I 0 − C A − 1 I ] [ A B C D ] [ I − A − 1 B 0 I ] = [ A 0 0 Δ A ] \left[ \begin{matrix} I& 0\\ -CA^{-1}& I \\ \end{matrix}\right] \left[ \begin{matrix} A& B\\ C& D \\ \end{matrix}\right] \left[ \begin{matrix} I& -A^{-1}B\\ 0& I \\ \end{matrix}\right] = \left[ \begin{matrix} A& 0\\ 0& \Delta A \\ \end{matrix}\right] \tag{4} [I−CA−10I][ACBD][I0−A−1BI]=[A00ΔA](4)
反过来，可以从对角形恢复 M M M：
(5) [ I 0 C A − 1 I ] [ A 0 0 Δ A ] [ I A − 1 B 0 I ] = [ A B C D ] \left[ \begin{matrix} I& 0\\ CA^{-1}& I \\ \end{matrix}\right] \left[ \begin{matrix} A& 0\\ 0& \Delta A \\ \end{matrix}\right] \left[ \begin{matrix} I& A^{-1}B\\ 0& I \\ \end{matrix}\right] = \left[ \begin{matrix} A& B\\ C& D \\ \end{matrix}\right] \tag{5} [ICA−10I][A00ΔA][I0A−1BI]=[ACBD](5)
舒尔补可以快速求解矩阵的逆
因为
(6) M = [ A B C D ] = [ I 0 C A − 1 I ] [ A 0 0 Δ A ] [ I A − 1 B 0 I ] M= \left[ \begin{matrix} A& B\\ C& D \\ \end{matrix}\right] = \left[ \begin{matrix} I& 0\\ CA^{-1}& I \\ \end{matrix}\right] \left[ \begin{matrix} A& 0\\ 0& \Delta A \\ \end{matrix}\right] \left[ \begin{matrix} I& A^{-1}B\\ 0& I \\ \end{matrix}\right] \tag{6} M=[ACBD]=[ICA−10I][A00ΔA][I0A−1BI](6)
所以
(7) M − 1 = [ A B C D ] = [ I − A − 1 B 0 I ] [ A − 1 0 0 Δ A − 1 ] [ I 0 − C A − 1 I ] M^{-1}= \left[ \begin{matrix} A& B\\ C& D \\ \end{matrix}\right] = \left[ \begin{matrix} I& -A^{-1}B\\ 0& I \\ \end{matrix}\right] \left[ \begin{matrix} A^{-1}& 0\\ 0& \Delta A^{-1} \\ \end{matrix}\right] \left[ \begin{matrix} I& 0\\ -CA^{-1}& I \\ \end{matrix}\right] \tag{7} M−1=[ACBD]=[I0−A−1BI][A−100ΔA−1][I−CA−10I](7)
3.舒尔补在多元高斯分布中的应用
3.1 多元变量的高斯分布
假设多元变量 x x x 服从高斯分布，且由两部分组成： x = [ a , b ] T x =[a,b]^T x=[a,b]T，变量之间构成之间的协方差矩阵为：
(8) K = [ A C T C D ] K = \left[ \begin{matrix} A& C^T\\ C& D \\ \end{matrix}\right] \tag{8} K=[ACCTD](8)
其中 A = c o v ( a , a ) , D = c o v ( b , b ) , C = c o v ( a , b ) A=cov(a,a) ,D = cov(b,b) , C = cov(a,b) A=cov(a,a),D=cov(b,b),C=cov(a,b)。所以变量 x x x的概率分布为
(9) P ( a , b ) = P ( a ) P ( b ∣ a ) ∝ e x p ( − 1 2 [ a b ] T [ A C T C D ] − 1 [ a b ] ) P(a,b) = P(a)P(b|a)\propto exp\left( -\frac{1}{2}\left[\begin{matrix} a\\ b\\ \end{matrix} \right]^T \left[ \begin{matrix} A& C^T\\ C& D \\ \end{matrix}\right]^{-1} \left[\begin{matrix} a\\ b\\ \end{matrix} \right] \right)\tag{9} P(a,b)=P(a)P(b∣a)∝exp(−21[ab]T[ACCTD]−1[ab])(9)
利用舒尔补对上式进行分解，则有
(10) P ( a , b ) ∝ e x p ( − 1 2 [ a b ] T [ A C T C D ] − 1 [ a b ] ) ∝ e x p ( − 1 2 [ a b ] T [ I − A − 1 C T 0 I ] [ A − 1 0 0 Δ A − 1 ] [ I 0 − C A − 1 I ] [ a b ] ) ∝ e x p ( − 1 2 [ a T ( b − C A − 1 a ) T ] [ A − 1 0 0 Δ A − 1 ] [ a b − C A − 1 a ] ) ∝ e x p ( − 1 2 ( a T A − 1 a ) + ( b − C A − 1 a ) T Δ A ( b − C A − 1 a ) ) ∝ e x p ( − 1 2 ( a T A − 1 a ) ) e x p ( − 1 2 ( b − C A − 1 a ) T Δ A ( b − C A − 1 a ) ) P(a,b) \propto exp\left( -\frac{1}{2}\left[\begin{matrix} a\\ b\\ \end{matrix} \right]^T \left[ \begin{matrix} A& C^T\\ C& D \\ \end{matrix}\right]^{-1} \left[\begin{matrix} a\\ b\\ \end{matrix} \right] \right) \\ \propto exp\left( -\frac{1}{2}\left[\begin{matrix} a\\ b\\ \end{matrix} \right]^T \left[ \begin{matrix} I& -A^{-1}C^T\\ 0& I \\ \end{matrix}\right] \left[ \begin{matrix} A^{-1}& 0\\ 0& \Delta A^{-1} \\ \end{matrix}\right] \left[ \begin{matrix} I& 0\\ -CA^{-1}& I \\ \end{matrix}\right] \left[\begin{matrix} a\\ b\\ \end{matrix} \right] \right) \\ \propto exp\left( -\frac{1}{2} \left[ \begin{matrix} a^T & (b-CA^{-1}a)^T\\ \end{matrix}\right] \left[ \begin{matrix} A^{-1}& 0\\ 0& \Delta A^{-1} \\ \end{matrix}\right] \left[ \begin{matrix} a\\ b-CA^{-1}a \\ \end{matrix}\right] \right) \\ \propto exp\left( -\frac{1}{2} (a^TA^{-1}a)+ (b-CA^{-1}a)^T\Delta A(b-CA^{-1}a) \right) \\ \propto exp\left( -\frac{1}{2} (a^TA^{-1}a)\right) exp\left(-\frac{1}{2} (b-CA^{-1}a)^T\Delta A(b-CA^{-1}a) \right) \\ \tag{10} P(a,b)∝exp(−21[ab]T[ACCTD]−1[ab])∝exp(−21[ab]T[I0−A−1CTI][A−100ΔA−1][I−CA−10I][ab])∝exp(−21[aT(b−CA−1a)T][A−100ΔA−1][ab−CA−1a])∝exp(−21(aTA−1a)+(b−CA−1a)TΔA(b−CA−1a))∝exp(−21(aTA−1a))exp(−21(b−CA−1a)TΔA(b−CA−1a))(10)
所以有 (11) P ( a ) = e x p ( − 1 2 ( a T A − 1 a ) ) P(a) =exp\left( -\frac{1}{2}(a^TA^{-1}a)\right)\tag{11} P(a)=exp(−21(aTA−1a))(11)
(12) P ( b ∣ a ) = e x p ( − 1 2 ( b − C A − 1 a ) T Δ A ( b − C A − 1 a ) ) P(b|a) =exp\left(-\frac{1}{2} (b-CA^{-1}a)^T\Delta A(b-CA^{-1}a) \right) \tag{12} P(b∣a)=exp(−21(b−CA−1a)TΔA(b−CA−1a))(12)
这意味着我们能从多元高斯分布 P ( a , b ) P(a,b) P(a,b)中分解得到边界概率 P ( a ) P(a) P(a)和条件概率P(b|a)。
3.2 边缘概率和条件概率的协方差矩阵
对于边缘概率 P ( a ) P(a) P(a)，有
(13) P ( a ) = ∫ P ( a , b ) d b P(a) = \int P(a,b)db\tag{13} P(a)=∫P(a,b)db(13)
(14) P ( a ) = e x p ( − 1 2 ( a T A − 1 a ) ) ∼ N ( 0 , A ) P(a) =exp\left( -\frac{1}{2}(a^TA^{-1}a)\right) \sim N(0,A)\tag{14} P(a)=exp(−21(aTA−1a))∼N(0,A)(14)
特点：边缘概率 P ( a ) P(a) P(a)的协方差就是从联合概率分布的协方差矩阵中取对应的矩阵块即可
对于条件概率 P ( b ∣ a ) P(b|a) P(b∣a)，有
(15) P ( b ∣ a ) = e x p ( − 1 2 ( b − C A − 1 a ) T Δ A ( b − C A − 1 a ) ) P(b|a) =exp\left(-\frac{1}{2} (b-CA^{-1}a)^T\Delta A(b-CA^{-1}a) \right) \tag{15} P(b∣a)=exp(−21(b−CA−1a)TΔA(b−CA−1a))(15)
特点：条件概率 P ( b ∣ a ) ∼ N ( C A − 1 a , Δ A ) P(b|a)\sim N(CA^{-1}a,\Delta A) P(b∣a)∼N(CA−1a,ΔA)，协方差为 a a a对应的舒尔补 Δ A \Delta A ΔA，均值为 C A − 1 a CA^{-1}a CA−1a。
3.3 边缘概率和条件概率的信息矩阵
信息矩阵是协方差矩阵的逆，所以变量 x x x的信息矩阵为
(16) K − 1 = [ A C T C D ] − 1 = [ Λ a a Λ a b Λ b a Λ b b ] K^{-1} = \left[ \begin{matrix} A& C^T\\ C& D \\ \end{matrix}\right] ^{-1} = \left[ \begin{matrix} \Lambda_{aa}& \Lambda_{ab}\\ \Lambda_{ba}& \Lambda_{bb} \\ \end{matrix}\right] \tag{16} K−1=[ACCTD]−1=[ΛaaΛbaΛabΛbb](16)
由公式(7)可知，信息矩阵与协方差矩阵元素之间的关系为
(17) K − 1 = [ A − 1 + A − 1 C T Δ A − 1 C A − 1 − A − 1 C T Δ A − 1 − Δ A − 1 C A − 1 Δ A − 1 ] = [ Λ a a Λ a b Λ b a Λ b b ] K^{-1} = \left[ \begin{matrix} A^{-1}+A^{-1}C^T\Delta A^{-1}CA^{-1}& -A^{-1}C^T\Delta A^{-1}\\ -\Delta A^{-1}CA^{-1}& \Delta A^{-1}\\ \end{matrix}\right] = \left[ \begin{matrix} \Lambda_{aa}& \Lambda_{ab}\\ \Lambda_{ba}& \Lambda_{bb} \\ \end{matrix}\right] \tag{17} K−1=[A−1+A−1CTΔA−1CA−1−ΔA−1CA−1−A−1CTΔA−1ΔA−1]=[ΛaaΛbaΛabΛbb](17)
由(14)知 边缘概率的协方差矩阵为 A A A，所以其对应的信息矩阵为 A − 1 A^{-1} A−1，根据式(17)可知
(18) A − 1 = A − 1 + A − 1 C T Δ A − 1 C A − 1 − ( − A − 1 C T Δ A − 1 ( Δ A − 1 ) − 1 − Δ A − 1 C A − 1 ) = Λ a a − Λ a b Λ b b − 1 Λ b a A^{-1} = A^{-1}+A^{-1}C^T\Delta A^{-1}CA^{-1}-(-A^{-1}C^T\Delta A^{-1}( \Delta A^{-1})^{-1} -\Delta A^{-1}CA^{-1}) =\Lambda_{aa}-\Lambda_{ab}\Lambda_{bb}^{-1}\Lambda_{ba}\tag{18} A−1=A−1+A−1CTΔA−1CA−1−(−A−1CTΔA−1(ΔA−1)−1−ΔA−1CA−1)=Λaa−ΛabΛbb−1Λba(18)
即边缘概率 P ( a ) P(a) P(a)的信息矩阵为 Λ a a − Λ a b Λ b b − 1 Λ b a \Lambda_{aa}-\Lambda_{ab}\Lambda_{bb}^{-1}\Lambda_{ba} Λaa−ΛabΛbb−1Λba。
由式(15)可知条件概率 P ( b ∣ a ) P(b|a) P(b∣a)的协方差矩阵为 Δ A \Delta A ΔA，所以其信息矩阵为 Δ A − 1 = Λ b b \Delta A^{-1} =\Lambda_{bb} ΔA−1=Λbb。
3.4总结
边际概率对于协方差矩阵的操作是很容易的，但不好操作信息矩阵。条件概率恰好相反，对于信息矩阵容易操作，不好操作协方差矩阵。
表格总结如下
4. 舒尔补在vslam中的应用
随着 VSLAM 系统不断往新环境探索，就会有新的相机姿态以及看到新的环境特征，最小二乘残差就会越来越多，信息矩阵越来越大，计算量将不断增加。 为了保持优化变量的个数在一定范围内，需要使用滑动窗口算法动态增加或移除优化变量。
但是该如何移除旧的状态变量呢？
直接丢弃变量和对应的测量值，会损失信息。正确的做法是使用边际概率，将丢弃变量所携带的信息传递给剩余变量。即根据舒尔补的在边缘概率方面得到的结论，从 P ( x 1 , x 2 , x 3 , . . . . . , x n ) P(x_{1},x_{2},x_{3},.....,x_{n}) P(x1,x2,x3,.....,xn)的协方差矩阵或信息矩阵中求得 P ( x 2 , x 3 , . . . . . , x n ) P(x_{2},x_{3},.....,x_{n}) P(x2,x3,.....,xn)的协方差矩阵或信息矩阵。该过程称之为边缘化。这是滑动窗口算法中非常重要的理论基础。
该部分内容还有很多细节，以后在继续补充。
智能推荐
艾美捷Epigentek DNA样品的超声能量处理方案-程序员宅基地
文章浏览阅读15次。空化气泡的大小和相应的空化能量可以通过调整完全标度的振幅水平来操纵和数字控制。通过强调超声技术中的更高通量处理和防止样品污染，Epigentek EpiSonic超声仪可以轻松集成到现有的实验室工作流程中，并且特别适合与表观遗传学和下一代应用的兼容性。Epigentek的EpiSonic已成为一种有效的剪切设备，用于在染色质免疫沉淀技术中制备染色质样品，以及用于下一代测序平台的DNA文库制备。该装置的经济性及其多重样品的能力使其成为每个实验室拥有的经济高效的工具，而不仅仅是核心设施。
11、合宙Air模块Luat开发：通过http协议获取天气信息_合宙获取天气-程序员宅基地
文章浏览阅读4.2k次，点赞3次，收藏14次。目录点击这里查看所有博文 本系列博客，理论上适用于合宙的Air202、Air268、Air720x、Air720S以及最近发布的Air720U（我还没拿到样机，应该也能支持）。 先不管支不支持，如果你用的是合宙的模块，那都不妨一试，也许会有意外收获。 我使用的是Air720SL模块，如果在其他模块上不能用，那就是底层core固件暂时还没有支持，这里的代码是没有问题的。例程仅供参考！..._合宙获取天气
EasyMesh和802.11s对比-程序员宅基地
文章浏览阅读7.7k次，点赞2次，收藏41次。1 关于meshMesh的意思是网状物，以前读书的时候，在自动化领域有传感器自组网，zigbee、蓝牙等无线方式实现各个网络节点消息通信，通过各种算法，保证整个网络中所有节点信息能经过多跳最终传递到目的地，用于数据采集。十多年过去了，在无线路由器领域又把这个mesh概念翻炒了一下，各大品牌都推出了mesh路由器，大多数是3个为一组，实现在面积较大的住宅里，增强wifi覆盖范围，智能在多热点之间切换，提升上网体验。因为节点基本上在3个以内，所以mesh的算法不必太复杂，组网形式比较简单。各厂家都自定义了组_802.11s
线程的几种状态_线程状态-程序员宅基地
文章浏览阅读5.2k次，点赞8次，收藏21次。线程的几种状态_线程状态
stack的常见用法详解_stack函数用法-程序员宅基地
文章浏览阅读4.2w次，点赞124次，收藏688次。stack翻译为栈，是STL中实现的一个后进先出的容器。要使用 stack，应先添加头文件include<stack>，并在头文件下面加上“ using namespacestd;"1. stack的定义其定义的写法和其他STL容器相同, typename可以任意基本数据类型或容器：stack<typename> name;2. stack容器内元素的访问..._stack函数用法
2018.11.16javascript课上随笔（DOM）-程序员宅基地
文章浏览阅读71次。<li> <a href = "“#”>-</a></li><li>子节点：文本节点（回车），元素节点，文本节点。不同节点树： 节点（各种类型节点）childNodes:返回子节点的所有子节点的集合，包含任何类型、元素节点（元素类型节点）：child。node.getAttribute(at...
随便推点
layui.extend的一点知识 第三方模块base 路径_layui extend-程序员宅基地
文章浏览阅读3.4k次。//config的设置是全局的layui.config({ base: '/res/js/' //假设这是你存放拓展模块的根目录}).extend({ //设定模块别名 mymod: 'mymod' //如果 mymod.js 是在根目录，也可以不用设定别名 ,mod1: 'admin/mod1' //相对于上述 base 目录的子目录}); //你也可以忽略 base 设定的根目录，直接在 extend 指定路径（主要：该功能为 layui 2.2.0 新增）layui.exten_layui extend
5G云计算：5G网络的分层思想_5g分层结构-程序员宅基地
文章浏览阅读3.2k次，点赞6次，收藏13次。分层思想分层思想分层思想-1分层思想-2分层思想-2OSI七层参考模型物理层和数据链路层物理层数据链路层网络层传输层会话层表示层应用层OSI七层模型的分层结构TCP/IP协议族的组成数据封装过程数据解封装过程PDU设备与层的对应关系各层通信分层思想分层思想-1在现实生活种，我们在喝牛奶时，未必了解他的生产过程，我们所接触的或许只是从超时购买牛奶。分层思想-2平时我们在网络时也未必知道数据的传输过程我们的所考虑的就是可以传就可以，不用管他时怎么传输的分层思想-2将复杂的流程分解为几个功能_5g分层结构
基于二值化图像转GCode的单向扫描实现-程序员宅基地
文章浏览阅读191次。在激光雕刻中，单向扫描（Unidirectional Scanning）是一种雕刻技术，其中激光头只在一个方向上移动，而不是来回移动。这种移动方式主要应用于通过激光逐行扫描图像表面的过程。具体而言，单向扫描的过程通常包括以下步骤：横向移动（X轴）： 激光头沿X轴方向移动到图像的一侧。纵向移动（Y轴）： 激光头沿Y轴方向开始逐行移动，刻蚀图像表面。这一过程是单向的，即在每一行上激光头只在一个方向上移动。返回横向移动： 一旦一行完成，激光头返回到图像的一侧，准备进行下一行的刻蚀。
算法随笔：强连通分量-程序员宅基地
文章浏览阅读577次。强连通：在有向图G中，如果两个点u和v是互相可达的，即从u出发可以到达v，从v出发也可以到达u，则成u和v是强连通的。强连通分量：如果一个有向图G不是强连通图，那么可以把它分成躲个子图，其中每个子图的内部是强连通的，而且这些子图已经扩展到最大，不能与子图外的任一点强连通，成这样的一个“极大连通”子图是G的一个强连通分量（SCC）。强连通分量的一些性质：（1）一个点必须有出度和入度，才会与其他点强连通。（2）把一个SCC从图中挖掉，不影响其他点的强连通性。_强连通分量
Django(2)|templates模板+静态资源目录static_django templates-程序员宅基地
文章浏览阅读3.9k次，点赞5次，收藏18次。在做web开发，要给用户提供一个页面，页面包括静态页面+数据，两者结合起来就是完整的可视化的页面，django的模板系统支持这种功能，首先需要写一个静态页面,然后通过python的模板语法将数据渲染上去。1.创建一个templates目录2.配置。_django templates
linux下的GPU测试软件,Ubuntu等Linux系统显卡性能测试软件 Unigine 3D-程序员宅基地
文章浏览阅读1.7k次。Ubuntu等Linux系统显卡性能测试软件 Unigine 3DUbuntu Intel显卡驱动安装，请参考：ATI和NVIDIA显卡请在软件和更新中的附加驱动中安装。 这里推荐： 运行后，F9就可评分，已测试显卡有K2000 2GB 900+分，GT330m 1GB 340+ 分，GT620 1GB 340+ 分，四代i5核显340+ 分，还有写博客的小盒子100+ 分。relaybot@re...