slam学习(1)——卡尔曼滤波

                                         卡尔曼滤波

作用

      根据机器人上个时刻的状态估计出这个时刻的机器人状态(估计值)。然后根据在这一时刻传感器的观测值，来纠正更新估计值，等到更精准的置信值

前提假设

• 1. 卡尔曼滤波算法认为当前状态与上一时刻的状态是线性关系，即

由于这个式子中包含噪声，所以无法准确的求出当前状态 ，但是我们可以求出 的概率分布，然后把这个概率分布的均值作为当前状态的估计，卡尔曼滤波就是根据这个思想，它算在当前的观测数据条件下的概率分布，然后吧这个概率分布的均值作为更准确的状态估计，加上当前观测数据是 ，则我们想求出的是 的概率分布，然后把这个概率分布的均值作为当前时刻的更准确的置信值。 是指在观测数据是， 已知上一个时刻状态置信值是, 控制变量是 的条件下的概率分布

• 2.  卡尔曼滤波还假设在k时刻的观测数据 和状态 之间的关系也是线性的，即
• 3. 卡尔曼滤波还加上这里面的两个噪声 和 都是服从均值为0 的高斯分布，即

    

   

其中R 和Q 是这两个概率分布的方差

• 4. 卡尔曼滤波假设上一时刻状态 服从高斯分布，即 ,当前k时刻状态 也服从高斯分布，但均值和方差待求，在k时刻的观测数据也服从高斯分布，待求

 

最终公式形式

声明:    是后验概率，  是先验概率,  是先验方差 ， 是后验方差

---

1. 

2. 

3.  

4.  

5.  

---

 

公式推导及理解

公式1，2 推导

有前提假设1: 及我们要求的是 可得

                                   

具体运算，参考高斯分布线性运算规则

在此可求得 公式1和2

   

公式3，4推导

根据多维高斯分布公式

(关注exp 后面部分)，

由于最终要求的是

则后验概率的二次项 是观测数据的二次项 加上先验概率二次项

 即，改变下形式，如下，其中

根据卡尔曼系数 K定义： 与系统总误差成正比，与测量总误差成反比，即：

代入公式可得公式3 

又根据

则   =

    

 根据SMW（Sherman-Morrison-Woodbury）恒等式

 

 最终得 公式4

公式5 推导

实际应用

例子1： 温度估计

房间的实际温度是 24 度，不会变化，即 A=1，B=0。
测量引入的误差 R=4e-4，测量误差 是0.5 即标准差=0.5， 方差Q=0.25，观测方程系数=1，初始温度估计为 x_update[0]=26 度，初始系统总
误差 P[0]=2.0。利用卡尔曼滤波算法计算房间实际温度

import numpy
import pylab

n_iter = 50 #number of iterations
sz = (n_iter,) # size of array
x = 24 # truth value (actual temperature)
R = 4e-4 # process variance
sigma = 0.5 
Q = 0.25 # estimate of measurement variance, change to see effect
# 构造观测数据的标准正太分布，均值是x， 标准差是sigma
z = numpy.random.normal(x,sigma,size=sz) # observations (normal about x, sigma=0.1)


x_update=numpy.zeros(sz)      # a posteri estimate of x
P=numpy.zeros(sz)         # a posteri error estimate
x_pre=numpy.zeros(sz) # a priori estimate of x
P_pre=numpy.zeros(sz)    # a priori error estimate
K=numpy.zeros(sz)         # gain or blending factor


x_update[0] = 26
P[0] = 2.0


for k in range(1,n_iter):

    x_pre[k] = x_update[k-1]
    P_pre[k] = P[k-1]+R
 
    K[k] = P_pre[k]/( P_pre[k]+Q )
    x_update[k] = x_pre[k]+K[k]*(z[k]-x_pre[k])
    P[k] = (1-K[k])*P_pre[k]


pylab.figure()
pylab.plot(z,'k+',label='noisy measurements') 
pylab.plot(x_update,'b-',label='a posteri estimate') 
pylab.axhline(x,color='g',label='truth value')
pylab.legend()
pylab.xlabel('Iteration')
pylab.ylabel('temperature')


pylab.figure()
valid_iter = range(1,n_iter) # P_pre not valid at step 0
pylab.plot(valid_iter,P_pre[valid_iter],label='a priori error estimate')
pylab.xlabel('Iteration')
pylab.ylabel('Temperature Variance')
pylab.setp(pylab.gca(),'ylim',[0,P[0]])
pylab.show()

  

 

 

 

参考网址：

https://zhuanlan.zhihu.com/p/158751412

https://blog.csdn.net/varyshare/article/details/93045704

高斯分布线性运算规则

https://www.zhihu.com/search?type=content&q=%E9%AB%98%E6%96%AF%E5%88%86%E5%B8%83%E7%BA%BF%E6%80%A7%E8%BF%90%E7%AE%97%E8%A7%84%E5%88%99