舒尔补在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)的协方差矩阵或信息矩阵。该过程称之为边缘化。这是滑动窗口算法中非常重要的理论基础。
该部分内容还有很多细节，以后在继续补充。
智能推荐
前端开发之vue-grid-layout的使用和实例-程序员宅基地
文章浏览阅读1.1w次，点赞7次，收藏34次。vue-grid-layout的使用、实例、遇到的问题和解决方案_vue-grid-layout
Power Apps-上传附件控件_powerapps点击按钮上传附件-程序员宅基地
文章浏览阅读218次。然后连接一个数据源，就会在下面自动产生一个添加附件的组件。把这个控件复制粘贴到页面里，就可以单独使用来上传了。插入一个“编辑”窗体。_powerapps点击按钮上传附件
C++ 面向对象（Object-Oriented）的特征 & 构造函数& 析构函数_"object(cnofd[\"ofdrender\"])十条"-程序员宅基地
文章浏览阅读264次。(1) Abstraction (抽象)(2) Polymorphism (多态)(3) Inheritance (继承)(4) Encapsulation (封装)_"object(cnofd[\"ofdrender\"])十条"
修改node_modules源码，并保存，使用patch-package打补丁，git提交代码后，所有人可以用到修改后的_修改 node_modules-程序员宅基地
文章浏览阅读133次。删除node_modules，重新npm install看是否成功。在 package.json 文件中的 scripts 中加入。修改你的第三方库的bug等。然后目录会多出一个目录文件。_修改 node_modules
【】kali--password:su的 Authentication failure问题，&sudo passwd root输入密码时Sorry, try again._password: su: authentication failure-程序员宅基地
文章浏览阅读883次。【代码】【】kali--password:su的 Authentication failure问题，&sudo passwd root输入密码时Sorry, try again._password: su: authentication failure
整理5个优秀的微信小程序开源项目_微信小程序开源模板-程序员宅基地
文章浏览阅读1w次，点赞13次，收藏97次。整理5个优秀的微信小程序开源项目。收集了微信小程序开发过程中会使用到的资料、问题以及第三方组件库。_微信小程序开源模板
随便推点
Centos7最简搭建NFS服务器_centos7 搭建nfs server-程序员宅基地
文章浏览阅读128次。Centos7最简搭建NFS服务器_centos7 搭建nfs server
Springboot整合Mybatis-Plus使用总结(mybatis 坑补充)_mybaitis-plus ruledataobjectattributemapper' and '-程序员宅基地
文章浏览阅读1.2k次，点赞2次，收藏3次。前言mybatis在持久层框架中还是比较火的，一般项目都是基于ssm。虽然mybatis可以直接在xml中通过SQL语句操作数据库，很是灵活。但正其操作都要通过SQL语句进行，就必须写大量的xml文件，很是麻烦。mybatis-plus就很好的解决了这个问题。..._mybaitis-plus ruledataobjectattributemapper' and 'com.picc.rule.management.d
EECE 1080C / Programming for ECESummer 2022 Laboratory 4: Global Functions Practice_eece1080c-程序员宅基地
文章浏览阅读325次。EECE 1080C / Programming for ECESummer 2022Laboratory 4: Global Functions PracticePlagiarism will not be tolerated:Topics covered:function creation and call statements (emphasis on global functions)Objective:To practice program development b_eece1080c
洛谷p4777 【模板】扩展中国剩余定理-程序员宅基地
文章浏览阅读53次。被同机房早就1年前就学过的东西我现在才学，wtcl。设要求的数为\(x\)。设当前处理到第\(k\)个同余式，设\(M = LCM ^ {k - 1} _ {i - 1}\) ，前\(k - 1\)个的通解就是\(x + i * M\)。那么其实第\(k\)个来说，其实就是求一个\(y\)使得\(x + y * M ≡ a_k(mod b_k)\)转化一下就是\(y * M ...
android 退出应用没有走ondestory方法,[Android基础论]为何Activity退出之后，系统没有调用onDestroy方法？...-程序员宅基地
文章浏览阅读1.3k次。首先，问题是如何出现的？晚上复查代码，发现一个activity没有调用自己的ondestroy方法我表示非常的费解，于是我检查了下代码。发现再finish代码之后接了如下代码finish();System.exit(0);//这就是罪魁祸首为什么这样写会出现问题System.exit(0);////看一下函数的原型public static void exit (int code)//Added ..._android 手动杀死app,activity不执行ondestroy
SylixOS快问快答_select函数 导致堆栈溢出 sylixos-程序员宅基地
文章浏览阅读894次。Q: SylixOS 版权是什么形式, 是否分为<开发版税>和<运行时版税>.A: SylixOS 是开源并免费的操作系统, 支持 BSD/GPL 协议(GPL 版本暂未确定). 没有任何的运行时版税. 您可以用她来做任何 您喜欢做的项目. 也可以修改 SylixOS 的源代码, 不需要支付任何费用. 当然笔者希望您可以将使用 SylixOS 开发的项目 (不需要开源)或对 SylixOS 源码的修改及时告知笔者.需要指出: SylixOS 本身仅是笔者用来提升自己水平而开发的_select函数 导致堆栈溢出 sylixos