舒尔补在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)的协方差矩阵或信息矩阵。该过程称之为边缘化。这是滑动窗口算法中非常重要的理论基础。
该部分内容还有很多细节，以后在继续补充。
智能推荐
基于内核4.19版本的XFRM框架_linux的xfrm框架-程序员宅基地
文章浏览阅读794次，点赞2次，收藏5次。XFRM框架_linux的xfrm框架
织梦常用标签整理_织梦中什么页面用什么标签教学-程序员宅基地
文章浏览阅读774次。DedeCMS常用标签讲解笔记整理 今天我们主要将模板相关内容，在前面的几节课中已经基本介绍过模板标签的相关内容，大家可以下载天工开物老师的讲课记录：http://bbs.dedecms.com/132951.html,这次课程我们主要讲解模板具体的标签使用，并且结合一些实例来介绍这些标签。 先前课程介绍了，网站的模板就如同一件衣服，衣服的好坏直接决定了网站的好坏，很多网站一看界面_织梦中什么页面用什么标签教学
工作中如何编译开源工具（gdb）_gdb编译-程序员宅基地
文章浏览阅读2.5k次，点赞2次，收藏15次。编译是大部分工程师的烦恼，大家普遍喜欢去写业务代码。但我觉得基本的编译流程，我们还是需要掌握的，希望遇到相关问题，不要退缩，尝试去解决。天下文章一大抄，百度能解决我们90%的问题。_gdb编译
python简易爬虫v1.0-程序员宅基地
文章浏览阅读1.8k次，点赞4次，收藏6次。python简易爬虫v1.0作者：William Ma (the_CoderWM)进阶python的首秀，大部分童鞋肯定是做个简单的爬虫吧，众所周知，爬虫需要各种各样的第三方库，例如scrapy, bs4, requests, urllib3等等。此处，我们先从最简单的爬虫开始。首先，我们需要安装两个第三方库：requests和bs4。在cmd中输入以下代码：pip install requestspip install bs4等安装成功后，就可以进入pycharm来写爬虫了。爬
安装flask后vim出现：error detected while processing /home/zww/.vim/ftplugin/python/pyflakes.vim:line 28_freetorn.vim-程序员宅基地
文章浏览阅读2.6k次。解决方法：解决方法可以去github重新下载一个pyflakes.vim。执行如下命令git clone --recursive git://github.com/kevinw/pyflakes-vim.git然后进入git克降目录，./pyflakes-vim/ftplugin,通过如下命令将python目录下的所有文件复制到~/.vim/ftplugin目录下即可。cp -R ...._freetorn.vim
HIT CSAPP大作业：程序人生—Hello‘s P2P-程序员宅基地
文章浏览阅读210次，点赞7次，收藏3次。本文简述了hello.c源程序的预处理、编译、汇编、链接和运行的主要过程，以及hello程序的进程管理、存储管理与I/O管理，通过hello.c这一程序周期的描述，对程序的编译、加载、运行有了初步的了解。_hit csapp
随便推点
挑战安卓和iOS！刚刚，华为官宣鸿蒙手机版，P40搭载演示曝光！高管现场表态：我们准备好了...-程序员宅基地
文章浏览阅读472次。点击上方 "程序员小乐"关注,星标或置顶一起成长后台回复“大礼包”有惊喜礼包！关注订阅号「程序员小乐」，收看更多精彩内容每日英文Sometimes you play a..._挑战安卓和ios!华为官宣鸿蒙手机版,p40搭载演示曝光!高管表态:我们准备好了
精选了20个Python实战项目(附源码)，拿走就用！-程序员宅基地
文章浏览阅读3.8w次，点赞107次，收藏993次。点击上方“Python爬虫与数据挖掘”，进行关注回复“书籍”即可获赠Python从入门到进阶共10本电子书今日鸡汤昔闻洞庭水，今上岳阳楼。大家好，我是小F。Python是目前最好的编程语言之一。由于其可读性和对初学者的友好性，已被广泛使用。那么要想学会并掌握Python，可以实战的练习项目是必不可少的。接下来，我将给大家介绍20个非常实用的Python项目，帮助大家更好的..._python项目
android在线图标生成工具,图标在线生成工具Android Asset Studio的使用-程序员宅基地
文章浏览阅读1.3k次。在网站的导航资源里看到了一个非常好用的东西：Android Asset Studio，可以在线生成各种图标。之前一直在用一个叫做Android Icon Creator的插件，可以直接在Android Studio的插件里搜索，这个工具的优点是可以生成适应各种分辨率的一套图标，有好几种风格的图标资源，遗憾的是虽然有很多套图标风格，毕竟是有限的。Android Asset Studio可以自己选择其..._在线 android 图标
android 无限轮播的广告位_轮播广告位-程序员宅基地
文章浏览阅读514次。无限轮播广告位没有录屏，将就将就着看，效果就是这样主要代码KsBanner.java/** * 广告位 * * Created by on 2016/12/20. */public class KsBanner extends FrameLayout implements ViewPager.OnPageChangeListener { private List
echart省会流向图（物流运输、地图）_java+echart地图+物流跟踪-程序员宅基地
文章浏览阅读2.2k次，点赞2次，收藏6次。继续上次的echart博客，由于省会流向图是从echart画廊中直接取来的。所以直接上代码<!DOCTYPE html><html><head> <meta charset="utf-8" /> <meta name="viewport" content="width=device-width,initial-scale=1,minimum-scale=1,maximum-scale=1,user-scalable=no" /&_java+echart地图+物流跟踪
Ceph源码解析：读写流程_ceph 发送数据到其他副本的源码-程序员宅基地
文章浏览阅读1.4k次。一、OSD模块简介1.1 消息封装：在OSD上发送和接收信息。cluster_messenger -与其它OSDs和monitors沟通client_messenger －与客户端沟通1.2 消息调度：Dispatcher类，主要负责消息分类1.3 工作队列：1.3.1 OpWQ: 处理ops（从客户端）和sub ops（从其他的OSD）。运行在op_tp线程池。1...._ceph 发送数据到其他副本的源码