舒尔补在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)的协方差矩阵或信息矩阵。该过程称之为边缘化。这是滑动窗口算法中非常重要的理论基础。
该部分内容还有很多细节，以后在继续补充。
智能推荐
JWT(Json Web Token)实现无状态登录_无状态token登录-程序员宅基地
文章浏览阅读685次。1.1.什么是有状态？有状态服务，即服务端需要记录每次会话的客户端信息，从而识别客户端身份，根据用户身份进行请求的处理，典型的设计如tomcat中的session。例如登录：用户登录后，我们把登录者的信息保存在服务端session中，并且给用户一个cookie值，记录对应的session。然后下次请求，用户携带cookie值来，我们就能识别到对应session，从而找到用户的信息。缺点是什么？服务端保存大量数据，增加服务端压力 服务端保存用户状态，无法进行水平扩展 客户端请求依赖服务.._无状态token登录
SDUT OJ逆置正整数-程序员宅基地
文章浏览阅读293次。SDUT OnlineJudge#include<iostream>using namespace std;int main(){int a,b,c,d;cin>>a;b=a%10;c=a/10%10;d=a/100%10;int key[3];key[0]=b;key[1]=c;key[2]=d;for(int i = 0;i<3;i++){ if(key[i]!=0) { cout<<key[i.
年终奖盲区_年终奖盲区表-程序员宅基地
文章浏览阅读2.2k次。年终奖采用的平均每月的收入来评定缴税级数的，速算扣除数也按照月份计算出来，但是最终减去的也是一个月的速算扣除数。为什么这么做呢，这样的收的税更多啊，年终也是一个月的收入，凭什么减去12*速算扣除数了？这个霸道（不要脸）的说法，我们只能合理避免的这些跨级的区域了，那具体是那些区域呢？可以参考下面的表格：年终奖一列标红的一对便是盲区的上下线，发放年终奖的数额一定一定要避免这个区域，不然公司多花了钱..._年终奖盲区表
matlab 提取struct结构体中某个字段所有变量的值_matlab读取struct类型数据中的值-程序员宅基地
文章浏览阅读7.5k次，点赞5次，收藏19次。matlab结构体struct字段变量值提取_matlab读取struct类型数据中的值
Android fragment的用法_android reader fragment-程序员宅基地
文章浏览阅读4.8k次。1，什么情况下使用fragment通常用来作为一个activity的用户界面的一部分例如, 一个新闻应用可以在屏幕左侧使用一个fragment来展示一个文章的列表,然后在屏幕右侧使用另一个fragment来展示一篇文章 – 2个fragment并排显示在相同的一个activity中,并且每一个fragment拥有它自己的一套生命周期回调方法,并且处理它们自己的用户输_android reader fragment
FFT of waveIn audio signals-程序员宅基地
文章浏览阅读2.8k次。FFT of waveIn audio signalsBy Aqiruse An article on using the Fast Fourier Transform on audio signals. IntroductionThe Fast Fourier Transform (FFT) allows users to view the spectrum content of _fft of wavein audio signals
随便推点
Awesome Mac：收集的非常全面好用的Mac应用程序、软件以及工具_awesomemac-程序员宅基地
文章浏览阅读5.9k次。https://jaywcjlove.github.io/awesome-mac/ 这个仓库主要是收集非常好用的Mac应用程序、软件以及工具，主要面向开发者和设计师。有这个想法是因为我最近发了一篇较为火爆的涨粉儿微信公众号文章《工具武装的前端开发工程师》，于是建了这么一个仓库，持续更新作为补充，搜集更多好用的软件工具。请Star、Pull Request或者使劲搓它 issu_awesomemac
java前端技术---jquery基础详解_简介java中jquery技术-程序员宅基地
文章浏览阅读616次。一.jquery简介 jQuery是一个快速的，简洁的javaScript库，使用户能更方便地处理HTML documents、events、实现动画效果，并且方便地为网站提供AJAX交互 jQuery 的功能概括1、html 的元素选取2、html的元素操作3、html dom遍历和修改4、js特效和动画效果5、css操作6、html事件操作7、ajax_简介java中jquery技术
Ant Design Table换滚动条的样式_ant design ::-webkit-scrollbar-corner-程序员宅基地
文章浏览阅读1.6w次，点赞5次，收藏19次。我修改的是表格的固定列滚动而产生的滚动条引用Table的组件的css文件中加入下面的样式：.ant-table-body{ &amp;::-webkit-scrollbar { height: 5px; } &amp;::-webkit-scrollbar-thumb { border-radius: 5px; -webkit-box..._ant design ::-webkit-scrollbar-corner
javaWeb毕设分享 健身俱乐部会员管理系统【源码+论文】-程序员宅基地
文章浏览阅读269次。基于JSP的健身俱乐部会员管理系统项目分享:见文末!
论文开题报告怎么写？_开题报告研究难点-程序员宅基地
文章浏览阅读1.8k次，点赞2次，收藏15次。同学们，是不是又到了一年一度写开题报告的时候呀？是不是还在为不知道论文的开题报告怎么写而苦恼?Take it easy!我带着倾尽我所有开题报告写作经验总结出来的最强保姆级开题报告解说来啦，一定让你脱胎换骨，顺利拿下开题报告这个高塔，你确定还不赶快点赞收藏学起来吗？_开题报告研究难点
原生JS 与 VUE获取父级、子级、兄弟节点的方法 及一些DOM对象的获取_获取子节点的路径 vue-程序员宅基地
文章浏览阅读6k次，点赞4次，收藏17次。原生先获取对象var a = document.getElementById("dom");vue先添加ref <div class="" ref="divBox">获取对象let a = this.$refs.divBox获取父、子、兄弟节点方法var b = a.childNodes; 获取a的全部子节点 var c = a.parentNode; 获取a的父节点var d = a.nextSbiling; 获取a的下一个兄弟节点 var e = a.previ_获取子节点的路径 vue