slam kf

一、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是线性系统的最优无偏估计。
二、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体验一下后端的例子。
粒子滤波器的原理与卡尔曼滤波有很大不同。