SLAM问题的处理方法主要分为滤波和图优化两类。
滤波的方法中常见的是扩展卡尔曼滤波、粒子滤波、信息滤波等，
熟悉滤波思想的同学应该容易知道这类SLAM问题是递增的、实时的处理数据并矫正机器人位姿。
比如基于粒子滤波的SLAM的处理思路是假设机器人知道当前时刻的位姿，
利用编码器或者IMU之类的惯性导航又能够计算下一时刻的位姿，
然而这类传感器有累计误差，所以再将每个粒子的激光传感器数据或者图像特征
对比当前建立好的地图中的特征，挑选和地图特征匹配最好的粒子的位姿当做当前位姿，如此往复。
当然在gmapping、hector_slam这类算法中，不会如此轻易的使用激光数据，
激光测距这么准，当然不能只用来计算粒子权重，
而是将激光数据与地图环境进行匹配（scan match）估计机器人位姿，比用编码器之流精度高出很多。
好了，扯了很多，目光回到今天的重心图优化。
目前SLAM主流研究热点几乎都是基于图优化的，在讲解之前，不免要问，为啥都用图优化了。
我想这和传感器有很大关系，以前使用激光构建二维的地图，现在研究热点都是用单目、双目、RGB-D构建地图。
处理视觉SLAM如果用EKF，随着时间推移地图扩大，内存消耗，计算量都很大；
而使用图优化计算在高建图精度的前提下效率还快。
关于filter 和 graph 两类方法在visual slam里的对比可以参见《visual slam: why filter?》，
lz在这方面没有深入学习对比，希望有能力的网友能够补充。
当然，不是说激光SLAM就不能用图优化，也可以，在博文中激光SLAM的图优化形式将以一个简单例子在博客中讲解。
在图优化的方法中（graph-based slam），处理数据的方式就和滤波的方法不同了，
它不是在线的纠正位姿，而是把所有数据记下来，最后一次性算账。
在这个graph slam tutorial系列中，还是熟悉的配方熟悉的味道，
和其他从推导到应用系列博文一样将通过简单的例子，
理论推导，程序应用等三个方面来介绍graph slam。
图是由节点和边构成，SLAM问题怎么构成图呢？在graph-based SLAM中，
机器人的位姿是一个节点（node）或顶点(vertex)，位姿之间的关系构成边(edge)。
具体而言比如t+1时刻和t时刻之间的odometry关系构成边，或者由视觉计算出来的位姿转换矩阵也可以构成边。
一旦图构建完成了，就要调整机器人的位姿去尽量满足这些边构成的约束。
所以图优化SLAM问题能够分解成两个任务：
1. 构建图，机器人位姿当做顶点，位姿间关系当做边，这一步常常被成为前端（front-end），往往是传感器信息的堆积。
2. 优化图，调整机器人位姿顶点尽量满足边的约束，这一步称为后端（back-end）。
图优化过程如下图所示：
先堆积数据，机器人位姿为构建的顶点。
边是位姿之间的关系，可以是编码器数据计算的位姿，
也可以是通过ICP匹配计算出来的位姿，还可以是闭环检测的位姿关系。
假设一个机器人初始起点在0处x0，然后机器人向前移动，通过编码器测得它向前移动了1m，到达第二个地点x1。
接着，又向后返回到x2，编码器测得它向后移动了0.8米。
但是，通过闭环检测，发现它回到了原始起点。
可以看出，编码器误差导致计算的位姿和观测到有差异，那机器人这几个状态中的位姿到底是怎么样的才最好的满足这些条件呢
首先构建位姿之间的关系，即图的边
线性方程组中变量小于方程的个数（三个未知数，四个方程），要计算出最优的结果，使出杀手锏最小二乘法。
先构建残差平方和函数：
为了使残差平方和最小，我们对上面的函数每个变量求偏导，并使得偏导数等于0.
整理得到
接着矩阵求解线性方程组
所以调整以后为满足这些边的条件，机器人的位姿为： X0 = 0, X1 = 0.93, X2 = 0.07.
在这里例子中我们发现，闭环检测起了决定性的作用。
上面是用闭环检测，这次用观测的路标（landmark）来构建边。如下图所示，假设一个机器人初始起点在0处，并观测到其正前方2m处有一个路标。然后机器人向前移动，通过编码器测得它向前移动了1m，这时观测到路标在其前方0.8m。请问，机器人位姿和路标位姿的最优状态
在这个图中，我们把路标也当作了一个顶点。构建边的关系如下
左边-右边 得到误差方程
残差平方和
求偏导数
最后整理并计算得
得到路标和机器人位姿 X0 = 0, X1 = 1.07, l2 = 1.93
将引入了一个重要的概念。我们知道传感器的精度是有差别的，也就是说我们对传感器的相信程度应该不同。比如假设这里编码器信息很精确，测得的路标距离不准，我们应该赋予编码器信息更高的权重，假设是10。
重新得到残差平方和如下
c = sum(fi) = x0^2 + 10 * (x1 - x0 - 1)^2 + (l0 - x0 -2)^2 + (l0 - x1 - 0.8)^2
求偏导得到
转换为矩阵求逆矩阵得到解 X0 = 0, X1 = 1.01, l2 = 1.9
将这个结果和之前对比，可以看到这里的机器人位姿x1更靠近编码器测量的结果。请记住这种思想，这里的权重就是在后面将要经常提到的边的信息矩阵。
由以上 实例分析可得：
目标函数
e 函数在原理上表示一个误差，是一个矢量，作为优化变量Xk和 实际值Zk符合程度的一个度量。
它越大表示xk越不符合zk。但是，由于目标函数必须是标量，所以必须用它的平方形式来表达目标函数。
最简单的形式是直接做成平方：e(x,z)转置 * e(x,z)。
进一步，为了表示我们对误差各分量重视程度的不一样，还使用一个信息矩阵 Ω 来表示各分量的不一致性。
信息矩阵 Ω 是协方差矩阵(ei 与 ej的相关性)的逆(即相关性越高，该误差项权重越小，或是 与该误差项(边)的变量的准确度相关)，是一个对称矩阵。由于图优化里每一条边代表一个测量值，如表示相邻位姿关系的编码器测量值 或者 图像（激光）匹配得到的位姿变换矩阵。所以图优化里每一条边的信息矩阵就是这些测量协防差矩阵的逆。如果协防差越小，表示这次测量越准越值得相信，信息权重就越大。
它的每个元素Ωi,j作为ei, ej的系数，可以看成我们对ei,ej这个误差项相关性的一个预计。
最简单的是把Ω设成对角矩阵（各误差间独立），对角阵元素的大小表明我们对此项误差的重视程度。
这里的Xk可以指一个顶点、两个顶点或多个顶点，取决于边的实际类型。
所以，更严谨的方式是把它写成ek(Zk,Xk1,Xk2,…)，但是那样写法实在是太繁琐，我们就简单地写成现在的样子。
由于Zk是已知的，为了数学上的简洁，我们再把它写成ek(Xk)的形式。
于是总体优化问题变为n条边加和的形式：
边的具体形式有很多种，可以是一元边、二元边或多元边，它们的数学表达形式取决于传感器或你想要描述的东西。
例如视觉SLAM中，在一个相机Pose Tk 处对空间点xk进行了一次观测，得到zk，那么这条二元边的数学形式即为:
单个边其实并不复杂。
可以使用 高斯牛顿法进行迭代优化，需要知道 两样东西：一个初始点和一个迭代方向。为了数学上的方便，先考虑第k条边ek(Xk)吧。
我们假设它的初始点为x˜k，并且给它一个Δx的增量，那么边的估计值就变为Fk(x˜k + Δx)，而误差值则从 ek(x˜k) 变为 ek(x˜k + Δx)。
首先对误差项进行一阶展开：
这是的Jk是ek关于Xk的导数，矩阵形式下为雅可比阵。
我们在估计点附近作了一次线性假设，认为函数值是能够用一阶导数来逼近的，当然这在Δx很大时候就不成立了。
于是，对于第条边的目标函数项，进一步展开：
其中，矩阵转置性质 (A * B)转置 = B转置 * A转置；(A + B)转置 = A转置 + B转置；(K * A)转置=K* A转置；A转置 * B = (B转置 * A)转置
在熟练的同学看来，这个推导就像(a+b)^2=a^2 + 2ab + b^2一样简单。
最后一个式子是个定义整理式，
我们把和Δx无关的整理成常数项 Ck，
把一次项系数写成 2 * bk ，
二次项则为 Hk（注意到二次项系数其实是Hessian矩阵），
bk = ek转置 * Ω * J ,
Hk = J转置* Ω * J 。
请注意 Ck 实际就是该边变化前的取值。所以在xk发生增量后，目标函数Fk项改变的值即为:
ΔFk = 2 * bk * Δx + Δx转置 * Hk * Δx
我们的目标是找到Δx，使得这个增量变为极小值（Fk最小 不发生变化）。
所以直接令它对于Δx的导数为零，有：
dFk / dΔx = 2 * b + 2 * Hk * Δx = 0 得到 Hk * Δx = −bk 所以归根结底，我们求解一个线性方程组：Hk * Δx = −bk，
Δx = - Hk逆 * bk = -(J转置 Ω * J)逆 * ek转置 * Ω * J*
*Δx = -(Hk + a * I) 逆 * bk = -(J转置 Ω * J + a * I )逆 * ek转置 * Ω * J ** 引入一个松弛因子来控制迭代速度
x* = x' + Δx，这里的加法不是简单的加法，是一个增量来更新 X ,这里需要指定更新方式。
如果把所有边放到一起考虑进去，那就可以去掉下标，直接说我们要求解 : H * Δx = −b.
b里面含有一个雅克比矩阵， Δx的系数为海塞矩阵
一阶导雅克比矩阵J 和 二阶导海塞矩阵 H 均为稀疏的矩阵，即大部分是零元素。 Jk = (0 ··· 0 Jk1 ··· Jki ··· 0 ··· Jkq0 ··· 0)
这种稀疏性能很好地帮助我们快速求解上面的线性方程。
稀疏代数库包括SBA、PCG、CSparse、Cholmod等等。
g2o正是使用它们来求解图优化问题的。
在数值计算中，我们可以给出雅可比和海塞的解析形式进行计算，也可以让计算机去数值计算这两个矩阵，而我们只需要给出误差的定义方式即可。
给目标函数F(x)一个增量Δx时，直接就写成了F(x+Δx)。但是这个加法可能没有定义！
最简单的就是常见的四维变换矩阵T 或者 三维旋转矩阵R。
它们对加法并不封闭，因为两个变换阵之和并不是变换阵，两个正交阵之和也不是正交阵。
它们乘法的性质非常好，但是确实没有加法，所以也不能像上面讨论的那样去求导。
虽然李群 SE(3) 和 SO(3) 是没有加法的，但是它们对应的李代数 se(3),so(3) 有啊！
我们可以求它们在正切空间里的流形上的梯度！ 我们就说，通过指数变换先把变换矩阵和旋转矩阵转换成李代数，在李代数上进行加法，
然后通过对数变换再转换到原本的李群中。这样我们就完成了求导。
核函数保证每条边的误差不会大的没边，剔除误差较大的边。
具体的方式是，把原先误差的二范数度量，替换成一个增长没有那么快的函数，同时保证自己的光滑性质（不然没法求导啊！）。
因为它们使得整个优化结果更为鲁棒，所以又叫它们为robust kernel（鲁棒核函数）。
1. 选择你想要的图里的节点与边的类型，确定它们的参数化形式；
2. 往图里加入实际的节点和边；
3. 选择初值，开始迭代；
4. 每一步迭代中，计算对应于当前估计值的雅可比矩阵J 和 海塞矩阵H；
5. 求解稀疏线性方程 Hk * Δx = −bk, 得到梯度方向Δx ；
6. 继续用 高斯牛顿GN 或 莱文贝格－马夸特方法LM 进行迭代。如果迭代结束，返回优化值。
实际上，g2o能帮你做好第3-6步，你要做的只是前两步而已。
系统预测状态 = 转换矩阵A * 系统上次状态 + 转换矩阵B * 系统输入 + 系统噪声w
1. 其中x是系统的状态向量，大小为n * 1 列向量 [n , 1]。
2. A为转换矩阵，大小为 n * n。
A*x_ ---> [n , n] * [n , 1] -----> [n , 1]
3. u为系统输入，大小为 k * 1。
4. B是将输入转换为状态的矩阵，大小为n * k。
B*u ---> [n , k] * [k , 1] -----> [n , 1]
5. w为系统噪声( 实际使用其协方差(多维)/方差 Q, 维度为[n, n] ), 大小为n * 1 列向量 [n , 1]。
注意这些矩阵的大小，它们与你实际编程密切相关。
有一个匀加速运动的小车，它受到的合力为 ft ,其位移速度方程如下：
0. 一个匀加速运动的小车，它受到的合力为 ft = m*a
1. 位移xt = 位移xt-1 + （速度vt + 加速度a* 时间t/2）* 时间t ；
2. 加速度a = 外力ft / 质量m ；
3. 速度vt = 速度vt-1 + 加速度a * 时间t
4. 该系统的状态向量包括位移和速度，分别用 xt 和 xt的导数 表示。
写成矩阵形式如下：
1. 其中系统状态 x 有两个二量，位移和速度，维度为 [2, 1]
2. 转换矩阵A大小为 [2, 2], 转换矩阵B大小为 [2, 1]。
3. 系统控制制输入变量为 u = f/m，也就是加速度,维度为[1, 1]，
4. 这里没有写出 系统噪声，其大小 和系统状态的维度相同，为 [2,1]
而其协方差矩阵R 维度为 [2，2]
1. 貌似有了这个模型就能完全估计系统状态了，速度能计算出，位移也能计算出。
2. 那还要卡尔曼干嘛，问题是很多实际系统复杂到根本就建不了模。
3. 并且，即使你建立了较为准确的模型，只要你在某一步有误差，
4. 由递推公式，很可能不断将你的误差放大A倍（A就是那个状态转换矩阵），
以至于最后得到的估计结果完全不能用了.
既然如此，我们就引进反馈。
从 概率论 贝叶斯模型 的观点 来看 前面预测 的 结果 就是 先验，测量出的结果就是后验。
系统测量值的预测zk = 转换矩阵H * 系统状态预测值x + 测量噪声R的方差v
预测的测量值是由系统状态变量映射出来的 zk = H*xk + v:
1. 其中zk是 系统测量值的预测值，大小为[m, 1](不是n * 1，也不是1 * 1，后面将说明），
2. H也是 系统状态变量 到 测量量的转换矩阵，大小为 [m, n]。
3. xK是 系统状态的预测值, 大小为 [n, 1]
H * xk -----> [m, n] * [n, 1] ---> [m, 1]
4. vk是测量过程噪声(实际使用其协方差(多维)/方差 R,维度 [m, m])，大小为[m, 1]。
同时对于匀加速模型，假设下车是匀加速远离我们，
我们站在原点用超声波仪器测量小车离我们的距离。
我们测量的是位移也就是xk，假设测量值等于xk，实际种是实际测量得到的数据
Zk = xk + 0*vk
写成 矩阵形式：
这里 因为只对位置量做了测量，所以其噪声的维度为 1*1
也就是测量值直接等于位移。
可能又会问，为什么不直接用测量值呢？
测量值噪声太大了，根本不能直接用它来进行计算。
试想一个本来是朝着一个方向做匀加速运动的小车，
你测出来的位移确是前后移动（噪声影响），只根据测量的结果，
你就以为车子一会往前开一会往后开。
对于状态方程中的 系统噪声w 和 测量噪声v，假设服从如下多元高斯分布，并且w,v是相互独立的。
噪声符合高斯分布，Q为w的协方差矩阵, R为v的协方差矩阵：
对于小车匀加速运动的模型，
假设系统的噪声向量只存在速度分量上，
且速度噪声的方差是一个常量0.01，
位移分量上的系统噪声为0。
只对位移进行测量，它的协方差矩阵R 大小是1*1，就是测量噪声的方差本身,
系统噪声协方差矩阵Q 为：
1. 系统噪声协方差矩阵Q中，的主对角线上为各变量自己的方差，
2. 叠加在速度上系统噪声方差为0.01，
3. 位移上的为0，
4. 它们间协方差为0，即噪声间没有关联。
系统估计值Xk = 系统状态方程预测值xk + 增益系数K * (真实测量值Zk - 测量值预测值zk)
1.理论预测（先验 Xk-1）有了，测量值（后验 测量值 Zk, 预测值zk = H*xk + v）也有了，
那怎么根据这两者得到最优的估计值呢？
2. 首先想到的就是加权(增益系数K )，或者称之为反馈。
3. 由一般的反馈思想, 设定值Zk 反馈值zk 做差 Zk - zk 之后 用比例系数K 放大，
考虑反馈，在加上原来预测值得到估计值：
1. 列出 系统估计值的 协方差矩阵 Pk-1 的公式
2. 求取 Pk-1 对角线 的和 （矩阵的迹，方差的和），求导等于0，使得估计值和真实值误差最小
3. 得到 增益 K = pk * H转置 * (H * pk * H转置 + R)逆
pk = A * Pk-1 * A转置 + Q
R为测量噪声的协方差矩阵
1. 系统 预测值(由历史数据的到)Xk' = A * Xk + Q ; Q为系统噪声的协方差
2. 上次 系统状态估计值 和 系统状态真实值间误差 的 协方差矩阵 Pk
3. 由协方差 的传递公式：
变量组X的线性变换， f(X) = AX，
假设X的协方差矩阵为C
则f(X)的协方差为 A * C * A转置 =
(A转置 * C逆 * A)逆 = (A转置 * C逆 * A)伪逆
4. xk = A * Xk-1 + w,
上次系统状态估计值 Xk-1 的协方差矩阵为Pk-1，系统噪声 w 的协方差矩阵为Q
得到本次 系统预测值Xk 的协方差矩阵
pk = A * Pk-1 * A转置 + Q
(相当于乘以系数的平方， 方差为 误差的平方, (x-均值)^2 , x放大了a倍，则(x-均值)^2放大a^2)
这里A 为矩阵，所以平方形式为 A * A转置
pk = A * Pk-1 * A转置 + Q
1. 系统状态 最后的估计值 XK = xk + K * （实际测量值Zk - 预测测量值zk） ， 反馈思想，见上文
其中，xk为系统预测值，
xk = A * Xk-1 + w (系统状态 过程传递),
zk = H * xk + v,
H，是状态变量到测量量的转换矩阵，
ZK 为实际测量数据。
2. 系统预测值 xk 的协方差矩阵 为 pk = A * Pk-1 * A转置 + Q
3. 系统估计值 Xk 的 协方差矩阵 Pk = pk - K * H * pk = ( I - K * H) * pk
式中，K 为 系统增益系数(反馈加权值，比例系数)
K = pk * H转置 * (H * pk * H转置 + R)逆
【1】首先要计算系统状态预测值xk、系统状态预测值和系统状态真实值之间误差 的 协方差矩阵pk。
1. 系统状态预测值 xk：
xk = A * Xk-1 + B * Uk + w ;
A， 为系统状态转移矩阵 ，
Xk-1，为系统状态 上次 估计值，
B， 为系统 输入转移矩阵，
Uk，为系统输入量,
w， 为系统噪声 .
2. 系统状态预测值的协方差 pk
pk = A * Pk-1 * A转置 + Q ; 协方差矩阵传递公式的到
A， 为系统状态转移矩阵 ，
Pk-1, 为上次系统状态估计值的 协方差 ,
Q， 为系统噪声的 协方差矩阵.
【2】计算卡尔曼增益 K，再然后得到 系统状态估计值 Xk
3. 卡尔曼增益 K
K = pk * H转置 * (H* pk * H转置 + R)逆 ;
pk，是系统状态预测值的协方差，
H， 是状态变量到测量量的转换矩阵，
R， 是测量噪声协方差。
4. 系统状态估计值 Xk
Xk = xk + K * （实际测量值ZK - 测量预测值zk ）
xk，系统状态预测值，
zk，是预测测量值，
zk = H * xk + v,
v， 是测量过程噪声
H，是状态变量到测量量的转换矩阵，
ZK 为实际测量数据。
【3】 最后还要计算 系统状态估计值和真实值之间的误差协方差矩阵，为下次递推做准备
5. 系统状态估计值 的 协方差 Pk
Pk = pk - K * H * pk = ( I - K * H) * pk
K，是卡尔曼增益， 系统增益系数(反馈加权值，比例系数)，
H， 是状态变量到测量量的转换矩阵，
pk，系统状态预测值的协方差
clc
clear all
close all
% 初始化参数
delta_t = 0.1; % 采样时间
t = 0:delta_t:5; % 时间范围
N = length(t); % 时间序列的长度
sz = [2,N]; % 信号需开辟的内存空间大小 2行*N列 2:为状态向量的维数n 位移 速度
g = 10; % 加速度值
x = 1/2*g*t.^2; % 实际真实位置序列 0×t + 1/2×g×t^2
z = x + sqrt(10).*randn(1,N); % 仿真的实际测量值
% 测量时加入测量白噪声 测量噪声v
% 方差R 为 10 均值为0
Q = [0 0;0 9e-1]; % 系统噪声协方差矩阵 假设建立的模型，噪声方差叠加在速度上 大小为n*n方阵 n=状态向量的维数=2
R = 10; % 位置测量协方差估计，可以改变它来看不同效果 m*m m=z(i)的维数，一个测量值
% n*n 状态 转移 矩阵 x = x * 1 + v * delta_t + 1/2*delta_t^2 *g ; v = x * 0 + v * 1 + delta_t*g
A = [1 delta_t;0 1]; % 系统状态转移矩阵
B = [1/2*delta_t^2;delta_t]; % 系统输入转移矩阵
H = [1,0]; % m*n 系统测量转移矩阵 z = 1 * x + 0 * v + 噪声
n = size(Q); % 系统状态维度数量 n为一个1*2的向量 Q为方阵
m = size(R); % 测量变量维度数量
% 分配空间
xhat = zeros(sz); % x的后验估计 状态估计值 Xk-1
P = zeros(n); % 后验方差估计 n*n 状态估计值的协方差 Pk-1
xhatminus = zeros(sz); % x的先验估计 状态 预测值 xk
Pminus = zeros(n); % n*n 状态 预测值 协方差 pk
K = zeros(n(1),m(1));% 卡尔曼增益 n*m n为系统状态数量 m为测量变量的维数
I = eye(n); % n*n 的 单位矩阵 对角矩阵
% 估计的初始值都为默认的0，即P=[0 0;0 0],xhat=0
% P=[2 0;0 2] //系统初始方差较大 算出来的 增益 K 就大，增益K 是测量真值 和 测量预测值 误差的系数 ，所以更相信测量值
for k = 9:N % 假设车子已经运动9个delta_T了，我们才开始估计
% 时间更新过程
xhatminus(:,k) = A * xhat(:,k-1) + B*g; %%%% 第一步：求系统预测值 xk 系统噪声均值为0 协方差 为 Q %%%%
% xk = A * Xk-1 + B * Uk + w ;
Pminus = A * P * A' + Q; %%%% 第二步：求系统预测值 的 协方差矩阵 %%%%
% pk = A * Pk-1 * A转置 + Q ;
% 测量更新过程
K = Pminus * H' * inv( H * Pminus*H'+ R); %%%% 第三步：计算卡尔曼增益 K %%%%
%K = pk * H转置 * (H* pk * H转置 + R)逆 ;
xhat(:,k) = xhatminus(:,k) + K*( z(k) - H * xhatminus(:,k));%%%% 第四步：计算测量系统状态估计值 Xk %%%%
% Xk = xk + K * （实际测量值ZK - 测量预测值zk ）
% zk = H * xk + v, v这里=0
P = ( I - K * H ) * Pminus; %%%% 第五步：计算估计值协方差的 协方差Pk %%%%
% Pk = pk + K * H * pk = ( I - K * H) * pk
end
figure
plot(t,z);
hold on
plot(t,xhat(1,:),'r-')
plot(t,x(1,:),'g-');
legend('含有噪声的测量', '后验估计', '真值');
xlabel('Iteration');