SLAM学习笔记2:Kalman Filter(卡尔曼滤波) 与Least Square(最小二乘法) 的比较
2016-03-04 18:56
351 查看
对于一个问题的解决,最根本在于怎样对它进行数学建模。对SLAM问题的建模,基本上是基于filter和graph两大类,今天整理了一下,对比两种模型的区别及共性。
主要参考这篇讲解:http://www.insidegnss.com/node/3445
以及《最优估计理论》刘胜,张红梅,科学出版社
Kalman filter和Least Square的目标都是误差最小化,Least Square是优化方法中的一种特殊情况,而卡尔曼滤波又是Least Square的一种特殊情况。
优化的目标是一个优化问题的关键,它决定了我们后续的算法选择。
常用的估计准则包括:
1. 无偏估计:即假设状态的估计值与真实值的平均值相等。
2. 最小二乘估计:不考虑数据的统计特性,如期望,方差等,直接用最小二乘法得到最优估计。
3. 误差方差最小:在满足最小二乘估计的同时,使得估计的误差方差最小。这一约束可以通过一系列等价的推导获得,前提是要事先知道测量数据噪声的方差。
简单来说就是,满足误差方差最小必满足误差平方和最小,反之不成立。而无偏估计是最基本假设。
由于最优估计里面变量比较多,自己看的时候基本上是长期保持懵逼态的,花了不少时间才理清楚。讲之前还是先明确几个概念吧,不分清楚后面基本就一锅粥了:
1. vi, 第i次测量时的噪声,测量噪声。
2. ei^, 第i次测量时,测量值与估计值之差,测量误差。
3. J(X^), ei^的平方和,最小二乘的最小化目标。
4. X~, 真实值X与估计值X^之差, 估计误差。
5. Var(X~), 估计误差的方差,误差方差最小化的最小化目标。
古典最小二乘估计(Ordinary Least Square, OLS)
假如有一个系统状态X,无法直接观测,那么用其它方式获得它的观测值 Z, Z=[Z1,Z2,...,Zn],若观测值是状态值的线性函数,则有:
Zi=HiX+Vi,i=1,2,...,n
Zi表示第i次测量得到的观测值, Hi表示第i次测量的观测模型, Vi表示第i次测量的误差。那么第i次测量的估计误差为:
ei^=Zi−HiX^
于是m次测量的总误差为:
J(X^)=∑i=1m(Zi−HiX^)2=(Z−HX^)T(Z−HX^)
我们的目标是要求总误差的最小值,导数必为0:
∂J(X^)∂X^=0
得到的X^LS 即是最小二乘估计值:
X^LS=(HTH)−1HTZ
此时估计误差为:
X~LS=X−X^LS=−(HTH)−1HTV
因为测量误差均值为0,所以估计误差的期望值为:
E(X~LS)=E[−(HTH)−1HTV]=−(HTH)−1HTE(V)=0
此时最小二乘估计为无偏估计,估计误差方差阵Var(X^) 与估计量的均方误差阵E[X−X^][X−X^]T 相等。
注意 J(X^LS) 与估计误差方差阵Var(X~LS) 概念不同,最小二乘估计只保证测量值与估计值的平方和最小,不保证估计误差的方差最小。最小二乘估计不需要随机变量V的任何统计信息。
加权最小二乘估计(Weighted Least Square, WLS)
在古典最小二乘中,假设了每一次测量的权重相同,但事实上这样并不合理。假如一个估计值偏离真实值很远,那么它对估计结果的影响就应该被削弱,反之影响应该加强。加权最小二乘就是做这样的事。
加权最小二乘估计的误差平方和为:
JW(X^)=(Z−HX^)TW(Z−HX^)
同样要使这个误差平方和最小化:
∂JW(X^)∂X^=0
求得:
X^LSW=(HTWH)−1HTWZ
注意此时状态估计值X^LSW与权重矩阵W相关,不像古典最小二乘的X^LS为定值,即相当于W为新增的一个自由度,我们可以根据需要设定一个目标,在这条轴上找到指定的点满足目标。于是我们以估计误差方差最小化为目标。
通过一系列推导可以发现,当W=R−1时,最小二乘估计是缺少初值条件下的线性无偏最小方差估计,又称马尔科夫估计。
至此最小二乘估计所做的都是批处理(Batch),这样的方法需要耗费大量内存存储所有历史数据,并且计算量很大。而且这不符合动态系统状态估计的需要,每一次接受到新测量值的时候,需要整个模型重新计算,不能在线估计(Online)。
递推最小二乘估计(Recursive Least Square, RLS)
继续考虑n次测量:
Zn=HnX+Vn
加权最小二乘估计为:
X^LSW(n)=[HTnRnHn]−1HTnR−1nHn
估计误差方差为:
Pk=E[X~LSW(n)X~TLSW(n)]
综合上面两式得:
X^LSW(n)=PnHTnR−1nZn
现在我们得到一个新测量:
zn+1=Hn+1X+vn+1
把新测量加入原来的大矩阵中得到:
Zn+1=Hn+1X+Vn+1
新的加权最小二乘估计为:
X^LSW(n+1)=[HTn+1Rn+1Hn+1]−1HTn+1R−1n+1Hn+1=Pn+1HTn+1R−1n+1Zn+1
要把新的测量噪声加入原本的测量噪声矩阵中,R矩阵应为对角矩阵:
R−1k+1=[R−1n00r−1n+1]
展开式子:
HTn+1R−1n+1Hn+1=[HTn,hTn+1][R−1n00r−1n+1][Hn,hn+1]=HTnR−1nHn+hTn+1r−1n+1hn+1
即:
P−1n+1=P−1n+hTn+1r−1n+1hn+1
最后推得:
Pn+1=Pn+PnhTn+1[hn+1PnhTn+1+rn+1]−1hn+1Pn
Kn+1=Pn+1hTn+1r−1n+1
X^LSW(n+1)=X^LSW(n)+Kn+1[zn+1−hn+1X^LSW(n)]
仔细观察发现,这货跟卡尔曼滤波的update式子就是一个意思啊。Weighted average of prediction and observation.
Graph SLAM
图优化SLAM就是基于权重最小二乘法的优化。所以每接收到一个新的观测值,必须记住还有information matrix需要更新。
Information matrix的更新方法:
首先明确information matrix是协方差矩阵的倒数。
1. 初始化时,I=0。
2. 对于每一个输入观测值,计算协方差(根据经验)。
3. 求出协方差矩阵的倒数,即为信息W,
4. 计算输出误差,把它对输入求偏导,得Jacobian矩阵J。
5. 把信息W转换到误差空间 X=JTWJ
6. 设置I=I+X
来源:http://www.triait.com/question/RGBD-SLAM-Compute-Information-Matrix/
卡尔曼滤波(Kalman Filter)
与递归最小二乘相似,卡尔曼滤波加入了系统内部变化的考虑。即利用process model对系统在下一时刻的状态进行预测。
关于卡尔曼滤波的详细讲解,白巧克力亦唯心 大神已经讲得很详细了,请直接参看原blog:
http://blog.csdn.net/heyijia0327/article/details/17487467
总结一下, 它们之间的关系简而言之就是:
1. 古典最小二乘只最小化测量误差平方和J(X^)。
2. 权重最小二乘不仅最小化测量误差平方和J(X^),还最小化估计误差方差阵X~。
3. 递归最小二乘考虑动态测量,解决权重最小二乘每一次接收到新测量值时,需要全局更新的问题。
4. 卡尔曼滤波不仅考虑动态测量,还考虑系统内部的动态变化,加入Process model进行系统状态预测,并将预测值与测量值做权重和,使测量误差平方和J(X^)最小,估计误差方差阵X~最小。
![](https://oscdn.geek-share.com/Uploads/Images/Content/202007/14/04a474acd91077d0dea6f5702397ae74)
![](https://oscdn.geek-share.com/Uploads/Images/Content/202007/14/ffa7951f8051f9862422d27d29e9d3c9)
来源:在Quora上看到的关于kalman与RLS区别的讨论:
https://www.quora.com/How-does-a-Kalman-filter-differ-from-Recursive-Least-Squares
既然二者优化的变量相同,为何图优化结果大大优于滤波?
根据与群中大神@吴博-东北大学 的讨论,虽然卡尔曼滤波与图优化所优化的变量一致,但是二者针对的问题却不一致,模型也不一样。
以下是个人思考,希望各路大神指正:
卡尔曼滤波重视的是预测值与观测值的最优融合问题,即对观测数据的测量误差最小化,此时递推算法即可达到需求。
而图优化要做的事情是从全局上看问题,假如加入图内的nodes和edges未能形成闭环,那图优化的结果应与递归最小二乘结果相同,仅优化测量误差。可是一旦回环检测成功,最小化误差的约束除了J(X^),Var(X~)最小以外,还需要增加T1T2...Tn=I, 这是一条很强的约束,把之前每一个估计值中包含的误差都尽可能地做了消除。而此时如果还使用递归算法的话,旧的观测值与状态值都被融合进了最近的上一个状态中,误差无法再通过batch方法最小化。所以致力于单步误差最小化的卡尔曼滤波无法解决地图一致性(consistency)的问题。
主要参考这篇讲解:http://www.insidegnss.com/node/3445
以及《最优估计理论》刘胜,张红梅,科学出版社
Kalman filter和Least Square的目标都是误差最小化,Least Square是优化方法中的一种特殊情况,而卡尔曼滤波又是Least Square的一种特殊情况。
优化的目标是一个优化问题的关键,它决定了我们后续的算法选择。
常用的估计准则包括:
1. 无偏估计:即假设状态的估计值与真实值的平均值相等。
2. 最小二乘估计:不考虑数据的统计特性,如期望,方差等,直接用最小二乘法得到最优估计。
3. 误差方差最小:在满足最小二乘估计的同时,使得估计的误差方差最小。这一约束可以通过一系列等价的推导获得,前提是要事先知道测量数据噪声的方差。
简单来说就是,满足误差方差最小必满足误差平方和最小,反之不成立。而无偏估计是最基本假设。
由于最优估计里面变量比较多,自己看的时候基本上是长期保持懵逼态的,花了不少时间才理清楚。讲之前还是先明确几个概念吧,不分清楚后面基本就一锅粥了:
1. vi, 第i次测量时的噪声,测量噪声。
2. ei^, 第i次测量时,测量值与估计值之差,测量误差。
3. J(X^), ei^的平方和,最小二乘的最小化目标。
4. X~, 真实值X与估计值X^之差, 估计误差。
5. Var(X~), 估计误差的方差,误差方差最小化的最小化目标。
古典最小二乘估计(Ordinary Least Square, OLS)
假如有一个系统状态X,无法直接观测,那么用其它方式获得它的观测值 Z, Z=[Z1,Z2,...,Zn],若观测值是状态值的线性函数,则有:
Zi=HiX+Vi,i=1,2,...,n
Zi表示第i次测量得到的观测值, Hi表示第i次测量的观测模型, Vi表示第i次测量的误差。那么第i次测量的估计误差为:
ei^=Zi−HiX^
于是m次测量的总误差为:
J(X^)=∑i=1m(Zi−HiX^)2=(Z−HX^)T(Z−HX^)
我们的目标是要求总误差的最小值,导数必为0:
∂J(X^)∂X^=0
得到的X^LS 即是最小二乘估计值:
X^LS=(HTH)−1HTZ
此时估计误差为:
X~LS=X−X^LS=−(HTH)−1HTV
因为测量误差均值为0,所以估计误差的期望值为:
E(X~LS)=E[−(HTH)−1HTV]=−(HTH)−1HTE(V)=0
此时最小二乘估计为无偏估计,估计误差方差阵Var(X^) 与估计量的均方误差阵E[X−X^][X−X^]T 相等。
注意 J(X^LS) 与估计误差方差阵Var(X~LS) 概念不同,最小二乘估计只保证测量值与估计值的平方和最小,不保证估计误差的方差最小。最小二乘估计不需要随机变量V的任何统计信息。
加权最小二乘估计(Weighted Least Square, WLS)
在古典最小二乘中,假设了每一次测量的权重相同,但事实上这样并不合理。假如一个估计值偏离真实值很远,那么它对估计结果的影响就应该被削弱,反之影响应该加强。加权最小二乘就是做这样的事。
加权最小二乘估计的误差平方和为:
JW(X^)=(Z−HX^)TW(Z−HX^)
同样要使这个误差平方和最小化:
∂JW(X^)∂X^=0
求得:
X^LSW=(HTWH)−1HTWZ
注意此时状态估计值X^LSW与权重矩阵W相关,不像古典最小二乘的X^LS为定值,即相当于W为新增的一个自由度,我们可以根据需要设定一个目标,在这条轴上找到指定的点满足目标。于是我们以估计误差方差最小化为目标。
通过一系列推导可以发现,当W=R−1时,最小二乘估计是缺少初值条件下的线性无偏最小方差估计,又称马尔科夫估计。
至此最小二乘估计所做的都是批处理(Batch),这样的方法需要耗费大量内存存储所有历史数据,并且计算量很大。而且这不符合动态系统状态估计的需要,每一次接受到新测量值的时候,需要整个模型重新计算,不能在线估计(Online)。
递推最小二乘估计(Recursive Least Square, RLS)
继续考虑n次测量:
Zn=HnX+Vn
加权最小二乘估计为:
X^LSW(n)=[HTnRnHn]−1HTnR−1nHn
估计误差方差为:
Pk=E[X~LSW(n)X~TLSW(n)]
综合上面两式得:
X^LSW(n)=PnHTnR−1nZn
现在我们得到一个新测量:
zn+1=Hn+1X+vn+1
把新测量加入原来的大矩阵中得到:
Zn+1=Hn+1X+Vn+1
新的加权最小二乘估计为:
X^LSW(n+1)=[HTn+1Rn+1Hn+1]−1HTn+1R−1n+1Hn+1=Pn+1HTn+1R−1n+1Zn+1
要把新的测量噪声加入原本的测量噪声矩阵中,R矩阵应为对角矩阵:
R−1k+1=[R−1n00r−1n+1]
展开式子:
HTn+1R−1n+1Hn+1=[HTn,hTn+1][R−1n00r−1n+1][Hn,hn+1]=HTnR−1nHn+hTn+1r−1n+1hn+1
即:
P−1n+1=P−1n+hTn+1r−1n+1hn+1
最后推得:
Pn+1=Pn+PnhTn+1[hn+1PnhTn+1+rn+1]−1hn+1Pn
Kn+1=Pn+1hTn+1r−1n+1
X^LSW(n+1)=X^LSW(n)+Kn+1[zn+1−hn+1X^LSW(n)]
仔细观察发现,这货跟卡尔曼滤波的update式子就是一个意思啊。Weighted average of prediction and observation.
Graph SLAM
图优化SLAM就是基于权重最小二乘法的优化。所以每接收到一个新的观测值,必须记住还有information matrix需要更新。
Information matrix的更新方法:
首先明确information matrix是协方差矩阵的倒数。
1. 初始化时,I=0。
2. 对于每一个输入观测值,计算协方差(根据经验)。
3. 求出协方差矩阵的倒数,即为信息W,
4. 计算输出误差,把它对输入求偏导,得Jacobian矩阵J。
5. 把信息W转换到误差空间 X=JTWJ
6. 设置I=I+X
来源:http://www.triait.com/question/RGBD-SLAM-Compute-Information-Matrix/
卡尔曼滤波(Kalman Filter)
与递归最小二乘相似,卡尔曼滤波加入了系统内部变化的考虑。即利用process model对系统在下一时刻的状态进行预测。
关于卡尔曼滤波的详细讲解,白巧克力亦唯心 大神已经讲得很详细了,请直接参看原blog:
http://blog.csdn.net/heyijia0327/article/details/17487467
总结一下, 它们之间的关系简而言之就是:
1. 古典最小二乘只最小化测量误差平方和J(X^)。
2. 权重最小二乘不仅最小化测量误差平方和J(X^),还最小化估计误差方差阵X~。
3. 递归最小二乘考虑动态测量,解决权重最小二乘每一次接收到新测量值时,需要全局更新的问题。
4. 卡尔曼滤波不仅考虑动态测量,还考虑系统内部的动态变化,加入Process model进行系统状态预测,并将预测值与测量值做权重和,使测量误差平方和J(X^)最小,估计误差方差阵X~最小。
来源:在Quora上看到的关于kalman与RLS区别的讨论:
https://www.quora.com/How-does-a-Kalman-filter-differ-from-Recursive-Least-Squares
既然二者优化的变量相同,为何图优化结果大大优于滤波?
根据与群中大神@吴博-东北大学 的讨论,虽然卡尔曼滤波与图优化所优化的变量一致,但是二者针对的问题却不一致,模型也不一样。
以下是个人思考,希望各路大神指正:
卡尔曼滤波重视的是预测值与观测值的最优融合问题,即对观测数据的测量误差最小化,此时递推算法即可达到需求。
而图优化要做的事情是从全局上看问题,假如加入图内的nodes和edges未能形成闭环,那图优化的结果应与递归最小二乘结果相同,仅优化测量误差。可是一旦回环检测成功,最小化误差的约束除了J(X^),Var(X~)最小以外,还需要增加T1T2...Tn=I, 这是一条很强的约束,把之前每一个估计值中包含的误差都尽可能地做了消除。而此时如果还使用递归算法的话,旧的观测值与状态值都被融合进了最近的上一个状态中,误差无法再通过batch方法最小化。所以致力于单步误差最小化的卡尔曼滤波无法解决地图一致性(consistency)的问题。
相关文章推荐
- 初学数模-MATLAB Quick Start! Part I
- 初学数模-MATLAB Quick Start! Part II
- 物流问题引发的思考:2013苏杯赛C题,26秒:3小时
- 封装好的Folyd建图,C++源码
- 最近我要数学建模了
- 数学建模有关DNA序列k-mer index的问题
- 2014年美国大学生数学建模竞赛翻译及建模思路
- SLAM 三维传感器 点滴
- 数学建模小议(第0章)
- 数学建模——层次分析法AHP
- 视觉SLAM漫谈
- matlab中关于fix(x),floor(x),ceil(x)的区别
- 数据分析的基本流程和方法
- 软件开发步骤
- 小白也玩数学建模--引
- SLAM和MonoSLAM
- 矩形切割
- 算法:ACM算法学习
- 2014年数学建模美赛题目原文及翻译[个人思路]