标签：状态 方程 后端 EKF 卡尔曼滤波 SLAM 概述 时刻 高斯分布
后端概述
（一）状态估计的概率解释
1、后端的概念
前端视觉里程计只能通过邻近的几张图片估计短暂时间内的运动轨迹和路标，这种方式为“渐进的”(Incremental)。
后端考虑一个更长时间内(或所有时间内)的状态估计问题,而且不仅使用过去的信息更新自己的状态,也会用未来的信息（即全局信息）来更新自己,这种处理方式为“批量的”(Batch)。
2、后端要解决的问题
（1）简单描述
由于方程中的观测量都受受噪声影响,所以要把这里的位姿 x 和路标 y 看成服从某种概率分布（一般为高斯分布）的随机变量,而不是单独的一个数。一般由于x和y的这一特点在程序中只需要储存它们的均值（变量估计值）和协方差矩阵（度量这一估计的不确定性）。
图 1 不确定性的直观描述。左侧:只有运动方程时,由于下一个时刻的位姿是在上一个时刻基础上添加了噪声,所以不确定性越来越大。右侧:存在路标点(红色)时,不确定性会明显减小。
因此后端要解决的问题就是：通过包含噪声的运动数据和观测数据,去估计状态量的高斯分布。在程序运
行期间,我们只要维护一个状态量,对它进行不断地迭代和更新即可。进一步,如果假设
状态量服从高斯分布,那我们只需考虑维护状态量的均值和协方差即可。
（2）数学表述
上式为简化版运动与观测方程，其中的x中包含了位姿和路标点两种未知量。则第k时刻的状态分布为： 
按照Bayes法则将k时刻状态分布估计分解为似然与先验两部分：
上式中似然可以由观测方程给定,而先验部分表示当前状态是基于过去所有的状态估计得来的。我们首先考虑xk关于他的前一个状态xk-1的先验（按照xk-1时刻为条件概率展开）：
而先验部分的后续处理分为两种路线：
- 以扩展卡尔曼滤波(EKF)为代表的滤波器方法：假设马尔可夫性,简单的一阶马氏性认为,k 时刻状态只与 k − 1 时刻状态有关,而与再之前的无关。
- 非线性优化为主体的优化框架：考虑 k 时刻状态与之前所有状态的关系。
（二）线性系统和KF
1、马尔可夫假设
考虑马尔可夫假设时：先验的第一部分可以简化为：
由于 k 时刻状态与 k − 1 之前的无关,所以就简化成只与 x k−1 和 u k 有关的形式,与 k 时刻的运动方程对应
第二部分可简化为：
考虑到 k 时刻的输入量 u k 与 k − 1 时刻的状态无关,所以我们把 u k 拿掉。可以看到,这一项实际是 k − 1 时刻的状态分布。
在程序运行期间,我们只要维护一个状态量,对它进行不断地迭代和更新即可。进一步,如果假设状态量服从高斯分布,那我们只需考虑维护状态量的均值和协方差即可。
2、线性高斯系统下的卡尔曼滤波器
运动和观测方程用线性高斯系统简化表示：
假设所有的状态和噪声均满足高斯分布。记这里的噪声服从零均值高斯分布:
- 卡尔曼滤波器的第一步,通过运动方程确定 x k 的先验分布。既下式的后边部分
根据高斯分布的性质,显然有:
其中x̂ k−1 和它的协方差 P̂ k−1第k-1时刻状态的后验估计（后验估计是指最后算得的准确分布）
- 卡尔曼滤波器的第二步，通过先验与似然估计x k的高斯分布，既计算xk～N ( x̂ k , P̂ k )中的x̂ k , P̂ k 值：
通过高斯分布指数项部分的系数对比获得x̂ k , P̂ k 。 - 上面的两个步骤可以归纳为“预测”(Predict)和“更新”(Update)两个步骤:
对与线性高斯系统，由于期间的高斯运算都是线性运算，故卡尔曼滤波器构成了线性系统的最优无偏估计。
（三）非线性系统和EKF
SLAM 中的运动方程和观测方程通常是非线性函数,尤其是视觉 SLAM 中的相机模型,需要使用相机内参模型以及李代数表示的位姿,更不可能是一个线性系统。由于高斯函数只有线性变换的性质，故需要先将非线性系统泰勒展开然后保留一阶或二阶来实现线性近似，再按照线性系统的高斯分布来计算，最后得到的就是扩展卡尔曼滤波器(Extended Kalman Filter, EKF)。
- 令 k −1 时刻的均值与协方差矩阵为 x̂ k−1 , P̂ k−1 。在 k 时刻,我们把运动方程和观测方程,在 x̂ k−1 , P̂ k−1处进行线性化(相当于一阶泰勒展开),有:
- 按照线性系统的卡尔曼滤波器的步骤，先“预测”：
得到先验的均值和协方差：
- 之后结合观测方程和Bayes 展开式,可以推导出 x k 的后验概率形式：
定义一个卡尔曼增益 K k :
在卡尔曼增益的基础上,后验概率的形式为:
与线性高斯系统不同，在 SLAM 这种非线性的情况下,EKF给出了单次线性近似下最大后验估计(MAP)，而不是无偏估计。
（四）滤波器后端小结
滤波器方法在早期slam中占主导地位，如今优化方法占主导且占有明显优势;但是在计算资源受限，或待估计量比较简单的场合，EKF也经常用。
EKF 的局限：
- 滤波器方法在一定程度上假设了马尔可夫性,有点像是在视觉里程计中,只考虑相邻两帧关系一样。如果当前帧确实与很久之前的数据有关(例如回环),那么滤波器就会难以处理这种情况。
- EKF 滤波器仅在 x̂ k−1 处做了一次线性化,然后就直接根据这次线性化结果,把后验概率给算了出来。而实际上,当我们离开近似点较远的时候,一阶泰勒展开并不一定能够近似整个函数,这取决于运动模型和观测模型的非线
性情况。如果它们有强烈的非线性,那线性近似就只在很小范围内成立,不能认为在很远的地方仍能用线性来近似。这就是 EKF 的非线性误差,是它的主要问题所在。 - 从程序实现上来说,EKF 需要存储状态量的均值和方差,并对它们进行维护和更新。存储量与状态量呈平方增长(因为要存储协方差矩阵n×n的矩阵)。因此,EKF SLAM 普遍被认为不适用于大型场景。
标签：状态,方程,后端,EKF,卡尔曼滤波,SLAM,概述,时刻,高斯分布 来源： https://blog.csdn.net/ljtx200888/article/details/112826138
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。