diff --git a/vSLAM/ch6/readme.md b/vSLAM/ch6/readme.md index c9c12807..99b91407 100644 --- a/vSLAM/ch6/readme.md +++ b/vSLAM/ch6/readme.md @@ -45,64 +45,342 @@ 边是位姿之间的关系，可以是编码器数据计算的位姿， 也可以是通过ICP匹配计算出来的位姿，还可以是闭环检测的位姿关系。 -# 一个帮助理解的例子 回环检测： -     为了更好的理解这个过程，将用一个很好的例子作说明。 - 如下图所示，假设一个机器人初始起点在0处，然后机器人向前移动， - 通过编码器测得它向前移动了1m，到达第二个地点。 - 接着，又向后返回，编码器测得它向后移动了0.8米。 - 但是，通过闭环检测，发现它回到了原始起点。 - 可以看出，编码器误差导致计算的位姿和观测到有差异，那机器人这几个状态中的位姿到底是怎么样的才最好的满足这些条件呢？ - 首先构建位姿之间的关系，即图的边： - 左-右=0 - 线性方程组中变量小于方程的个数，要计算出最优的结果，使出杀手锏最小二乘法。 - 先构建残差平方和函数： - 为了使残差平方和最小，我们对上面的函数每个变量求偏导，并使得偏导数等于0. +# 最小二乘图优化 +## 闭环检测 +> 假设一个机器人初始起点在0处x0，然后机器人向前移动，通过编码器测得它向前移动了1m，到达第二个地点x1。 - 整理得到： +> 接着，又向后返回到x2，编码器测得它向后移动了0.8米。 - 接着矩阵求解线性方程组： +> 但是，通过闭环检测，发现它回到了原始起点。 - 所以调整以后为满足这些边的条件，机器人的位姿为： +> 可以看出，编码器误差导致计算的位姿和观测到有差异，那机器人这几个状态中的位姿到底是怎么样的才最好的满足这些条件呢 +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/3.png) - 在这里例子中我们发现，闭环检测起了决定性的作用。 +> 首先构建位姿之间的关系，即图的边 +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/4.png) +> 线性方程组中变量小于方程的个数（三个未知数，四个方程），要计算出最优的结果，使出杀手锏最小二乘法。先构建残差平方和函数： +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/5.png) +> 为了使残差平方和最小，我们对上面的函数每个变量求偏导，并使得偏导数等于0. +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/6.png) +> 整理得到 +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/7.png) +> 接着矩阵求解线性方程组 +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/8.png) -另一个例子 通过路标构成回环： -  前面是用闭环检测，这次用观测的路标（landmark）来构建边。 - 如下图所示，假设一个机器人初始起点在0处，并观测到其正前方2m处有一个路标。 - 然后机器人向前移动，通过编码器测得它向前移动了1m，这时观测到路标在其前方0.8m。 - 请问，机器人位姿和路标位姿的最优状态？ +> 所以调整以后为满足这些边的条件，机器人的位姿为： +X0 = 0, X1 = 0.93, X2 = 0.07. - 在这个图中，我们把路标也当作了一个顶点。构建边的关系如下： +> 在这里例子中我们发现，闭环检测起了决定性的作用。 - 即 +## 观测的路标（landmark） +> 上面是用闭环检测，这次用观测的路标（landmark）来构建边。如下图所示，假设一个机器人初始起点在0处，并观测到其正前方2m处有一个路标。然后机器人向前移动，通过编码器测得它向前移动了1m，这时观测到路标在其前方0.8m。请问，机器人位姿和路标位姿的最优状态 +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/11.png) - 残差平方和： +> 在这个图中，我们把路标也当作了一个顶点。构建边的关系如下 +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/12.png) - 求偏导数： +> 左边-右边 得到误差方程 +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/13.png) - 最后整理并计算得： +> 残差平方和 +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/14.png) - 得到路标和机器人位姿： +> 求偏导数 +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/15.png) - 权重信息矩阵 +> 最后整理并计算得 +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/16.png) - 接下来，将引入了一个重要的概念。我们知道传感器的精度是有差别的，也就是说我们对传感器的相信程度应该不同。 - - 比如假设这里编码器信息很精确，测得的路标距离不准，我们应该赋予编码器信息更高的权重，假设是10。重新得到残差平方和如下： +> 得到路标和机器人位姿 +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 - 将这个结果和之前对比，可以看到这里的机器人位姿x1更靠近编码器测量的结果。 - 请记住这种思想，这里的权重就是在后面将要经常提到的边的信息矩阵，在后面还将介绍。 +> 求偏导得到 +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/17.png) + +> 转换为矩阵求逆矩阵得到解 +X0 = 0, X1 = 1.01, l2 = 1.9 + +> 将这个结果和之前对比，可以看到这里的机器人位姿x1更靠近编码器测量的结果。请记住这种思想，这里的权重就是在后面将要经常提到的边的信息矩阵。 + +# 图优化理论分析 +[图优化理论分析1](http://blog.csdn.net/heyijia0327/article/details/47731631) +[图优化理论分析2](http://www.cnblogs.com/gaoxiang12/p/5244828.html) + > 由以上 实例分析可得： + + > 目标函数 + +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/21.png) + > 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条边加和的形式： + +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/29.png) + +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/22.png) + +> 边的具体形式有很多种，可以是一元边、二元边或多元边，它们的数学表达形式取决于传感器或你想要描述的东西。 + +> 例如视觉SLAM中，在一个相机Pose Tk 处对空间点xk进行了一次观测，得到zk，那么这条二元边的数学形式即为: + +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/23.png) + +> 单个边其实并不复杂。 + +> 可以使用 高斯牛顿法进行迭代优化，需要知道 两样东西：一个初始点和一个迭代方向。为了数学上的方便，先考虑第k条边ek(Xk)吧。 + +> 我们假设它的初始点为x˜k，并且给它一个Δx的增量，那么边的估计值就变为Fk(x˜k + Δx)，而误差值则从 ek(x˜k) 变为 ek(x˜k + Δx)。 + +> 首先对误差项进行一阶展开： + +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/24.png) + +> 这是的Jk是ek关于Xk的导数，矩阵形式下为雅可比阵。 + +> 我们在估计点附近作了一次线性假设，认为函数值是能够用一阶导数来逼近的，当然这在Δx很大时候就不成立了。 + +> 于是，对于第条边的目标函数项，进一步展开： + +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/25.png) + +> 其中，矩阵转置性质 (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的系数为海塞矩阵 + +> 一阶导雅克比矩阵 和二阶导海塞矩阵 均为稀疏的矩阵，即大部分是零元素。 Jk = (0 ··· 0 Jk1 ··· Jki ··· 0 ··· Jkq0 ··· 0) + +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/30.png) + +> 这种稀疏性能很好地帮助我们快速求解上面的线性方程。稀疏代数库包括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. 每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵； + 5. 求解稀疏线性方程Hk * Δx = −bk, 得到梯度方向Δx ； + 6. 继续用 高斯牛顿GN 或 莱文贝格－马夸特方法LM进行迭代。如果迭代结束，返回优化值。 +　 　实际上，g2o能帮你做好第3-6步，你要做的只是前两步而已。 + + +# 卡尔曼滤波器 +[卡尔曼滤波](http://blog.csdn.net/heyijia0327/article/details/17487467) + +[卡尔曼滤波 -- 从推导到应用(一)](https://blog.csdn.net/heyijia0327/article/details/17487467) + +## 线性系统的状态差分方程   系统预测  = A 转换矩阵1 * 上次状态  + B 转换矩阵2 * 输入  + Q 噪声1 +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/41.png) + +> 其中x是系统的状态向量，大小为n * 1 列。A为转换矩阵，大小为 n * n。u为系统输入，大小为 k * 1。B是将输入转换为状态的矩阵，大小为n * k。随机变量w为系统噪声。注意这些矩阵的大小，它们与你实际编程密切相关。 + +### 看一个具体的匀加速运动的实例 +> 有一个匀加速运动的小车，它受到的合力为 ft , 由匀加速运动的位移和速度公式，能得到由 t-1 到 t 时刻的位移和速度变化公式 +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/42.png) + +> 位移x = 位移x + （速度v + 加速度* 时间/2）* 时间 ； 加速度 = 外力 f / 质量 m ； 速度v = 速度v + 加速度 * 时间 + +> 该系统的状态向量包括位移和速度，分别用 xt 和 xt的导数 表示。控制输入变量为u=f/m，也就是加速度，于是有如下矩阵形式 +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/44.png) + +> 这里对应的的矩阵A大小为 2 * 2 , 矩阵B大小为 2 * 1。 + +> 貌似有了这个模型就能完全估计系统状态了，速度能计算出，位移也能计算出。那还要卡尔曼干嘛，问题是很多实际系统复杂到根本就建不了模。并且，即使你建立了较为准确的模型，只要你在某一步有误差，由递推公式，很可能不断将你的误差放大A倍（A就是那个状态转换矩阵），以至于最后得到的估计结果完全不能用了。 + +> 既然如此，我们就引进反馈。从概率论贝叶斯模型的观点来看前面预测的结果就是先验，测量出的结果就是后验。 + +## 测量值的预测   =  H 转换矩阵3 * 系统预测值 + R 噪声2 +> 测量值是由系统状态变量映射出来的: +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/45.png) + +> 注意Z是测量值，大小为m * 1(不是n * 1，也不是1 * 1，后面将说明），H也是状态变量到测量量的转换矩阵。大小为m * n。随机变量v是测量噪声。 + +>同时对于匀加速模型，假设下车是匀加速远离我们，我们站在原点用超声波仪器测量小车离我们的距离。 + +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/46.png) + +> 也就是测量值直接等于位移。可能又会问，为什么不直接用测量值呢？测量值噪声太大了，根本不能直接用它来进行计算。试想一个本来是朝着一个方向做匀加速运动的小车，你测出来的位移确是前后移动（噪声影响），只根据测量的结果，你就以为车子一会往前开一会往后开。 +对于状态方程中的系统噪声w和测量噪声v，假设服从如下多元高斯分布，并且w,v是相互独立的。其中Q系统噪声,R为测量噪声变量的协方差矩阵。 + +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/47.png) + +> 对于小车匀加速运动的模型，假设系统的噪声向量只存在速度分量上，且速度噪声的方差是一个常量0.01，位移分量上的系统噪声为0。 + +> 测量值只有位移，它的协方差矩阵R 大小是1 * 1，就是测量噪声的方差本身, 而系统噪声协方差矩阵为： + +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/48.png) + +> Q中，叠加在速度上系统噪声方差为0.01，位移上的为0，它们间协方差为0，即噪声间没有关联。 + + +## 系统估计值 = 系统状态方程预测值 + 增益系数 * (真实测量值 - 测量值预测值) +> 理论预测（先验）有了，测量值（后验）也有了，那怎么根据这两者得到最优的估计值呢？首先想到的就是加权，或者称之为反馈。由一般的反馈思想我们得到估计值： +![](https://github.com/Ewenwan/Mathematics/tree/master/pic/49.png) + +## 增益系数的求解   +> 列出 系统估计值的 协方差矩阵 Pk+1 的公式 包含 增益 K 和 系统预测值 的协方差矩阵 Pk' 测量转移矩阵 H  测量过程噪声协方差 R +> 求取 Pk+1 对角线 的和 （矩阵的迹，方差的和），求导等于0，使得估计值和真实值误差最小 +> 得到 增益 K = Pk' * H转置 * (H* Pk' * H转置 + R)逆 + +## 系统预测值 的协方差矩阵   Xk' = A * Xk  + Q ; Q为系统噪声协方差 +> 上次 估计值和真实值间误差的协方差矩阵 Pk +> 由协方差的传递公式得到   本次 系统预测值Xk' 的协方差矩阵 Pk' = A * Pk * A转置 + Q(相当于乘以系数的平方，方差为 误差的平方) + + +## 系统估计值的 协方差矩阵   Xk+1 = Xk' + K * （真实测量值 - H * Xk' ） +> 系统预测值 Xk' 的协方差矩阵 为 Pk' +> 系统估计值 Xk+1 的 协方差矩阵 Pk+1 = Pk' + K * H * Pk' =  ( I + K * H) * Pk' + + +里总结下递推的过程，理一下思路： + + 【1】首先要计算预测值、预测值和真实值之间误差协方差矩阵。 + 预测值       Xk' = A * Xk  + B * Uk  + Q ; A 为系统状态转移矩阵 ， B为系统输入转移矩阵 Q为系统噪声协方差 + 预测值协方差 Pk' = A * Pk * A转置  + Q   ; Pk 为上次估计值的协方差 + 【2】计算卡尔曼增益K，再然后得到估计值 + 卡尔曼增益   K = Pk' * H转置 * (H* Pk' * H转置 + R)逆 ; H 为测量转移矩阵， R为测量噪声协方差 + 估计值       Xk+1 = Xk' + K * （真实测量值 - 测量预测值 ）= Xk+1 = Xk' + K * （真实测量值 - H * Xk' ） + 【3】 最后还要计算估计值和真实值之间的误差协方差矩阵，为下次递推做准备 + 估计值协方差 Pk+1 = Pk' + K * H * Pk' =  ( I + K * H) * Pk' + +## matlab 小车匀加速 程序示例： + + 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; % 实际真实位置序列 + z = x + sqrt(10).*randn(1,N); % 仿真 的 测量值 测量时加入测量白噪声 测量噪声 方差为 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的后验估计       状态估计值 + P=zeros(n);           % 后验方差估计 n*n  状态估计值 协方差 + xhatminus=zeros(sz); % x的先验估计       状态 预测值  + Pminus=zeros(n);     % n*n               状态 预测值 协方差 + K=zeros(n(1),m(1));   % Kalman增益 n*m   n 为 系统状态数量 m为 测量变量的维数 + I=eye(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; // 系统噪声均值为0  协方差 为 Q + Pminus= A * P * A' + Q; + + % 测量更新过程 + K = Pminus*H'*inv( H*Pminus*H'+R ); + xhat(:,k) = xhatminus(:,k)+K*(z(k)-H*xhatminus(:,k));   // 测量 噪声均值为0  协方差 为 R + P = (I-K*H)*Pminus;   + end + + figure + plot(t,z); + hold on + plot(t,xhat(1,:),'r-') + plot(t,x(1,:),'g-'); + legend('含有噪声的测量', '后验估计', '真值'); + xlabel('Iteration'); + + + + + + + +# 粒子滤波器 +[粒子滤波](http://blog.csdn.net/heyijia0327/article/details/40899819) + +[粒子滤波通俗解释](https://blog.csdn.net/x_r_su/article/details/53083438) + +[Particle Filter Tutorial 粒子滤波：从推导到应用（一）](https://blog.csdn.net/heyijia0327/article/details/40899819)