slam kf

1、KF

1.从几率来理解几率估计
由于但愿整个运动估计较长时间内最优，因此反而会用最新的知识去更新以前的状态，就好比在作完当前帧的位姿估计的时候，修改局部地图的路标点。
若是站在以前的状态上来考虑，用的是将来的信息。
因此不只用过去的信息估计如今的状态，也用将来的信息更新本身。由于本身站在一个中间的时间点上。这是批量式 batch
渐进式：只考虑过去和前一个时刻的。incremental
1.1前言
运动方程：
xk=f(xk-1,uk)+wk;
因为xk表示的位姿，它和上一时刻之间的位姿是运动关系。而带观测值就是观测方程了。
观测方程：
zk,j=h(yj,xk)+vk,j
yj是路标点，具体的zk,j能够是观测方向啊，或者位姿之类的。因为路标点不少，因此观测方程会比较多，远远多于运动方程。
若是没有测量运动的装置的时候，好比没有里程计，这时候只能单纯的经过图像来求解位姿了。这时候能够有几种处理方式好比假设没有运动方程，假设相机不动，假设相机匀速运动。不知道具体这种是怎么作的。相似于sfm问题,就是structure from motion，经过一组图像来恢复运动和结构，但slam图像有时间顺序。

若是假设状态量和噪声都服从高斯分布，那么只要在程序中存储均值和协方差阵就能够了。
均值：变量最优值的估计，协方差阵：度量偏差的，也就是不肯定性的。
只有运动方程不行，由于偏差会累积。没有讲只有观测值会怎样。
1.2改记号
xk={xk,y1,y2,...,ym}用它来表示k时刻的因此未知量，就是位姿和m个路标点，路标点是未知的吗？
zk,j=zk
因此原来的方程变成了
xk=f(xk-1,uk)+wk
zk=h(xk)+vk
比以前难理解，就是把路标点给隐匿了，必定程度上会让人难以理解。
虽然是zk=什么什么，但其实zk是已知的，可是yj是未知的。
由于xk包含了全部的未知量，因此要估计xk的状态分布。用的是0到k的数据，用几率表示就是
P(xk|x0,u1:k,z1:k)已知量是uk,zk,因此是u1:k,z1:k,当初始化的时候是x0.而后加u1,z1才有x1,u是从时刻1开始输入的。
因此是P(xk|x0,u1:k,z1:k)
按照贝叶斯估计，把这个几率变成
先展开
P(xk|x0,u1:k,z1:k)=P(xkx0u1:kz1:k)/P(x0,u1:k,z1:k)
分子分母提出zk来
=P(zk|xk,x0,u1:k,z1:k-1)*P(xk,x0,u1:k,z1:k-1)/（P(zk|x0,u1:k,z1:k-1)*P(x0,u1:k,z1:k-1))
分子的后一项和分母的后一项相除
=P(zk|xk,x0,u1:k,z1:k-1)*P(xk|x0,u1:k,z1:k-1)/P(zk|x0,u1:k,z1:K-1)
不知道为何把P(zk|xk,x0,u1:k,z1:k-1)/P(zk|x0,u1:k,z1:k-1)类比于P(zk|xk)
因此整个式子类比于
P(zk|xk)*P(xk|x0,u1:k,z1:k-1)
P(xk|x0,u1:k,z1:k)是后验来着，由于后验执果索因，根据观测方程，zk=h(xk)+vk.zk是由xk获得的，而如今求zk已经发生了以后xk的几率，这是后验。
而P(xk|x0,u1:k,z1:k-1),z1:k-1是根据以前的x1:k-1获得的，u1:k是以前的输入，因此这些都是xk以前的数据，根据以前的数据估计k时刻xk发生的几率，这是先验。
后验等于先验*似然。
P(zk|xk)是似然来着。
先验按xk-1展开，就是把几率变成积分形式
P(xk|x0,u1:k,z1:k-1)=(P(xk|xk-1,x0,u1:k,z1:k-1)P(xk-1|x0,u1:k,z1:k-1)dxk-1)的积分形式
这里是假设k时刻只和k-1时刻状态有关，但若是考虑更早时刻的，还能够展开。
一阶马尔科夫性：k只和k-1有关，滤波方法采用了这个假设。以EKF为主。
非线性优化是认为跟全部时刻有关。slam主流使用非线性优化。
2.线性系统,kf
kf来时候，k时刻状态只和k-1时刻有关。积分公式里面的两个几率
P(xk|xk-1,x0,u1:k,z1:k-1)=P(xk|xk-1,uk),这里的uk能够拿掉，它其实就是k-1时刻的状态分布做为输入来计算xk.
P(xk-1|x0,u1:k,z1:k-1)=P(xk-1|x0,u1:k-1,z1:k-1)其实也没有简化，不明白为何不把x0省掉，只去掉uk.
只须要把k-1时刻推导到k时刻就能够了。
线性高斯
xk=Akxk-1+uk+wk;
zk=Ckxk+vk
假设全部的状态和噪声都服从于高斯分布，wk~N(0,R),vk~N(0,Q)，R,Q省略了下标。
假设知道xk-1的预测值xk-1.hat(),Pk-1.hat
那么根据运动方程先验P(xk|x0,u1:k,z1:k-1)=N(Akxk-1.hat+uk,Ak.*Pk-1.hat()*Ak.t+R)
计算的时候要加上常数uk.
均值是常数都要加，协方差才忽略常数。
这样先验xk.g,pk.g就都知道了。
P(zk|xk)比较困惑的是，这里xk的状态分布不知道，这里写的是
P(zk|xk)=N(Ckxk,R)
即便把ckxk当作常数，后面的协方差阵也应该是Q，而不是R，不明白为何这么设。
后验服从于N(xk.hat,Pk.hat)
因此N(xk.hat,Pk.hat)=N(Ckxk,R)*N(xk.g,Pk.g)
N(xk.hat,Pk.hat)都是xk服从的，N(Ckxk,R)是zk服从的。因此
(xk-xk.hat).t*Pk.hat.n(xk-xk.hat)=(zk-Ckxk).t*R.n*(zk-Ckxk)+(xk-xk.g).t*Pk.g.n*(xk-xk.g)
中间乘的是协方差的逆。
比较xk的二次项系数
Pk.hat.n=Ck.t*R.n*Ck+Pk.g.n
等式同乘Pk.hat,得
I=Pk.hat*Ck.t*R.n*Ck+Pk.hat*Pk.g.n
为了写着方便，把等式右边第一项的前三个定义为K.即K=Pk.hat*Ck.t*R.n
I=K*Ck+Pk.hat*Pk.g.n
因此Pk.hat=(I-KCk).Pk.g,后验协方差就求出来了。
也就是说K跟后验，观察方程系数Ck,观察方程噪声偏差R有关。后验就是跟z有关嘛。
比较xk的一次项系数
Pk.hat.n*(-xk.hat)-xk.hat.t*Pk.hat.n=zk.t*R.n*(-Ck)-Ck.t*R.n*zk-xk.g.t*Pk.g.n-Pk.g.n*xk.g
Pk.hat.n*xk.hat=Ck.t*R.n*zk+Pk.g.n*xk.g
等式两边同乘Pk.hat
xk.hat=Pk.hat*Ck.t*R.n*zk+Pk.hat*Pk.g.n*xk.g
=K*zk+PK.hat*Pk.g.n*xk.g
而I=K*CK+Pk.hat*Pk.g.n
因此Pk.hat*Pk.g.n=I-K*Ck
因此
xk.hat=K*zk+(I-K*Ck)*xk.g
提取K得=xk.g+K(zk-Ck*xk.g)
因此xk.hat=xk.g+K(zk-Ckxk.g)
Pk.hat=(I-KCk)*Pk.g
因此迭代能够这么来写
1.预测
求出xk.g,pk.g
xk.g=Akxk-1.hat+uk,Pk.g=AkPk-1.hat*Ak.t+R
为了形成循环，这里用k-1的后验来求k的先验。Ak.t要放在后面
2.更新，先计算K
K=Pk.g*Ck.t(CKPk.gCk.t+RK).n
记住公式就好了，能够用其余方式推导出来的。K 能够不用Pk.hat获得。
而后计算xk.hat,Pk.hat,用于下一次的迭代。
xk.hat=xk.g+K(zk-Ckxk.g)
Pk.hat=(I-KCk)*Pk.g
对，更新以后去求下一次的值。
这就是kf了，kf是线性系统的最优无偏估计。
2、EKF
slam非线性系统，由于李代数。高斯分布通过非线性变换，再也不是高斯分布。取近似，把非高斯分布近似于一个高斯分布。
EKF，这个扩展主要指的是到非线性系统的扩展。在某个点附近，作一阶线性近似
原方程
xk=f(xk-1,uk)+wk
zk=h(xk)+vk
近似后
f(xk-1,uk)=f(xk-1.hat,uk)+f'(xk-1.hat,uk)*(xk-1-xk-1.hat)+wk
h(xk)=h(xk.g)+h'(xk.g)(xk-xk.g)+sk
记f'(xk-1.hat,uk)=F,h'(xk.g)=H
也就是xk=Fxk-1-F*xk-1.hat+f(xk-1.hat,uk)+wk
zk=Hxk-Hxk.g+h(xk.g)+vk
这样就相似于以前的线性高斯系统
xk=Axk-1+uk+wk
zk=Ckxk+vk
那么这样计算下来
xk.g=f(xk-1.hat,uk),Pk.g=FPk-1.hat*F.t+Rk
更新的时候
Kk=Pk.g*H.t*(HPk.g*H.t+Qk).n

xk.hat=xk.g+Kk(zk-h(xk.g))
Pk.hat=(I-Kk*H)*Pk.g
以前线性高斯系统xk.hat,Pk.hat能够说是无偏最优估计，这里是单次线性近似的最大后验估计(MAP)
1.讨论
想要在某段时间内估计某个不肯定量，ekf.早期slam用ekf.
IF（信息滤波器），IKF（iterated KF),UKF(unscented KF)，粒子滤波器，SWF（sliding window Filter),或者使用分治法来提升EKF的效率。
优点：计算量受限，待估计量比较简单的场合。
缺点：（1）马尔科夫性，不能使用全部历史数据，出现回环不能处理。
（2）若是运动模型和观测模型有强烈的非线性，那么线性近似只能在很小的范围内成立，在很远的地方则不成。
非线性优化：每迭代一次，状态改变以后，从新泰勒展开，滤波只在固定点上一次。因此状态变化很是大依然适用。至于ekf为何在固定点上只一次，不懂。

 

 

 

 

 

 

10.1.3非线性系统 ,Ekf
slam的运动方程和观测方程一般是非线性函数，尤为是slam中的相机模型，须要使用内参和李代数，exp（李代数）更不多是一个线性系统。一个高斯分布，通过非线性变换，每每再也不是一个高斯分布，取必定的近似，将以非高斯分布近似成一个高斯分布。
卡尔曼滤波扩展到非线性系统，ekf.一般的作法是，在某个点附近考虑运动方程和观测方程的一阶泰勒展开，只保留一阶项，即线性的部分，而后按照线性系统进行推导。k-1时刻,xk-1.hat,Pk-1.hat
在k时刻，运动方程，观测方程在xk-1.hat,pk-1.hat为何不是xk-1,pk-1是由于其实没有真值，只有估计值。由于都不能给出实际位姿吧。
一阶泰勒展开。
xk约等于f(xk-1.hat,uk)+f对于xk-1的求导代入xk-1.hat后求得的值*(xk-1-xk-1.hat)+wk
其实就是xk=f(xk-1.hat,uk)+f'(xk-1.hat,uk)*(xk-1-xk-1.hat)+wk
奥，懂了，xk=f(xk-1,uk)它展开后就是f(xk-1.hat,uk)+f'(xk-1.hat,uk)*(Xk-1-Xk-1.hat)+wk
记这里的偏导数为F.
则xk约等于f(xk-1.hat,uk)+F(Xk-1-Xk-1.hat)
一样对于观测方程，也有
zk约等于h(xk.g)+h'(xk.g)(xk-xk.g)+nk
这里的偏导数设成H.
为何是偏导，zk除了xk未知量，没有其余的了。
那么，在预测步骤中，根据运动方程有
P(xk|x0,u1:K,z0:k-1)=N(f(xk-1.hat,uk),FPk-1.hatF.t+Rk)
奥，这里F就是Ak,H就是Ck.
这些推导和kf是十分类似的，为方便记述，记这里的先验和协方差的均值为
xk.g=f(xk-1.hat,uk),Pk.g=FPk-1.hat*F.t+Rk
不能彻底按公式推断。xk=f(xk-1,uk)+wk
因此xk.g=f(xk-1.hat,uk)就能够了。
而后，考虑在观测中，咱们有
P(zk|xk)=N(h(xk.g)+H(xk-xk.g),Qk)
也就是xk已经存在要保留xk.可是方差为何是QK.
EKF的预测与更新
Kk=Pk.g*H.t(HPk.g*H.t+Qk).n
xk.hat=xk.g+Kk(zk-h(xk.g)),pk.hat=(I-KkH).Pk.g
其实跟以前的还差很少，只不过H代替Ck,F梯度Ak.
slam非线性的状况下，它给出了单次线性近似下的最大后验估计(MAP)
10.1.1ekf的讨论
ekf以形式简洁，应用普遍著称。当想要在某段时间内估计某不肯定量的时候，首先想到ekf.在早期的slam中，ekf占据了很长一段时间的主导地位。研究者们讨论了各类各样的滤波器在slam中的应用，好比if(信息滤波器。IKF iterated kf,UKF(unscented KF)无气味，例子滤波器，SWF（sliding window filter滑动窗口滤波器，或者采用分治法等思路改进ekf的效率。直至今日，尽管咱们认识到非线性优化比滤波器占有明显的优点，可是在计算资源受限，或者待估计量比较简单的场合，仍是会用ekf.
局限：
1.马尔科夫性的局限，若是当前帧确实跟好久以前的数据有关，好比回环的状况，那么滤波器就很难处理了。非线性优化，full slam.
2.ekf滤波器仅在xk-1.hat处作了一次线性化，而后就根据此次线性结果，算后验几率。这至关于在说，咱们认为该点处的线性化近似在后验几率处还是有效的。而实际上，当咱们离开工做点较远时，一阶泰勒展开并不能近似整个函数。这取决于运动模型和观测模型的非线性状况。若是它们有强烈的非线性，那么线性近似就只在很小范围内成立。不能认为在很远的地方仍能用线性来近似。这就是ekf的非线性偏差，也是它的主要问题所在。
在优化问题中，尽管咱们也作一阶（最速降低）或二阶（高斯牛顿法或列温伯格-马夸尔特方法）的近似，但每迭代一次，状态估计发生变化以后，咱们会从新对新的估计点作泰勒展开，而不像ekf那样只在固定点上作一次泰勒展开，这就使得优化方法适用范围更广，在状态变化较大时亦能适用。
3.从程序实现上来讲，efk须要存储均值和方差，并对它们进行维护和更新。若是把路标量也放进状态，因为视觉slam中路标数量很大，这个存储量是至关可观的，且与状态量呈平方增加，由于要存储协方差矩阵。所以ekf slam被广泛认为不适用于大型场景。
因为ekf存在这些明显的缺点，同等计算量的状况下，非线性优化能取得更好的效果。以非线性优化为主的后端。主要介绍图优化，并用g2o和ceres体验一下后端的例子。
粒子滤波器的原理与卡尔曼滤波有很大不一样。