卡尔曼滤波

作者: 飞多多 | 来源:发表于2019-09-28 21:07 被阅读0次

卡尔曼滤波及其无人驾驶应用
图文并茂，卡曼滤波
iOS-卡尔曼滤波算法
卡尔曼滤波
卡尔曼滤波
卡尔曼滤波
卡尔曼滤波
卡尔曼滤波
卡尔曼滤波
卡尔曼滤波

话不多说，我这里先给出我们的系统的模型方程，状态转移方程：

$x_k=Ax_{k-1}+u_k+w$

测量方程：

$z_k=Cx_k+v$

需要说明的是，这里的 $A、C、w、v$ 也可以是随 $k$ 变化的，但是为了简便，也符合绝大部分的工程应用实际，我们假定它们是固定的常数和时平稳，并不影响对结果的讨论。其中 $w\sim N(0,R)$ ， $v\sim N(0,Q)$
然后我这里给出相应的滤波过程:

预测
$\bar{x}_k=A_k\hat{x}_{k-1}+u_k\label{eq:1}\\ \bar{P}_k =A_k\hat{P}_{k-1}{A_k}^T+R$
更新
$K=\bar{P}_k{C}^T{(C\bar{P}_k{C}^T+Q)}^{-1}\\ \hat{x}_k=\bar{x}_k+K(z_k-C\bar{x}_k)\\ \hat{P}_k=(I-KC)\bar{P}_k$
这里也有几点说明。变量上面带一横的表示是先验值，变量上面带一个尖帽子的表示后验值，也就是待求的最优值。 $P_k$ 是相应的协方差矩阵。

我们知道，在统计概率学中，如果有随机变量A和B和常量c，那么我们有(D(A+B)=D(A)+D(B)+2Cov(A,B)) 和(D(cA)=c^2D(A))。然后我们接下来一步一步的理解滤波的过程。

考虑第 $k-1$ 次，我们已经有了最有值 $\hat{x}_{k-1}$ 以及对应的协方差矩阵 $\hat{P}_{k-1}$ ，根据状态转移方程和前文的协方差的公式，我们自然的能从第一个方程中得出先验预测值 $\bar{x}_k$ ，从第二个方程得出 $\bar{P}_k$ 。到这里我们完成了预测。单纯依靠这个预测值，我们能大概知道(k)时刻的状态，但是不够精确,如果单依靠模型的预测，随着时间的进行，预测值的方差将发散。

与此同时，我们又进行了测量，实际测量值是 $z_k$ ，此时，我们对下一步的估计值有两个备选：1.相信模型的预测。2.相信传感器的测量。选哪个好呢？卡尔曼说，综合二者，共同考察，使得最后的估计既不发散（单依靠估计会发散），也不出现异常值（单依靠测量，会出现异常值，且缺少模型信息）。但是我们该如何综合考察二者呢？答案是加权平均。如何加权呢？答案是，谁的方差小，谁的权就大。

为了直观的理解起见，在以下讨论中，我们假定C是个单位阵，事实上，实际工程测量中，存在不少的情况，C就是单位阵。具体的操作就是上述公式中计算卡尔曼增益那一步，K其实是测量的权，（I-K）是预测的权，如果我们把更新的第二步写成 $\hat{x}_k=(I-K)\bar{x}_k+Kz_k$ ，这样看起来就直观得多。K的求解公式标量形式形如 $k = \frac{\bar{P}_k}{\bar{P}_k+Q}$ ，这就是根据方差的占比来决定其权的大小，只不过矩阵形式采用的是矩阵逆的形式。

然后预测值与测量值加权求和就是我们的最优估计值了。也就是我们更新的第二步，求最优估计值。

最后为了下一次的迭代，我们要计算当前最优估计值的方差。以下讨论仍然在标量形式下，由最优估计值的计算公式，我们知道 $\hat{P}_k = \bar{P}_k + K^2(Q-\bar{P}_k)$ ，我们反解更新的第一步，得到 $Q = \frac{\bar{P}_k-K\bar{P}_k}{K}$ ，将其带入，我们得到， $\hat{P}_k=(I-KC)\bar{P}_k$ ，至此，我们得到了形式上的一致。

虽然以上的讨论大都假定在标量形式下，且C=1，但是这并步妨碍我们对卡尔曼原理的直观理解。我们可以对其做不严格的推广，推广到矩阵。事实上，我们可以严格的证明卡尔曼的更新原理。

我们的卡尔曼滤波中，Q与R是需要们来调节的。如果是Q、R是常量，最好的当然是根据样本统计来计算Q和R了。如果是关于时刻k的函数，那就麻烦得多了。

最后，我给出一个我的演示小程序

import numpy as np
import matplotlib.pyplot as plt

def kalman_filter(measured,last_optimal,last_optimal_convarance):
    Q = 1.0     #measure
    R = 0.5     #drive
    drived = last_optimal + 0.1
    P = last_optimal_convarance + R     #variance of drived
    K = P/(P+Q) 
    print "K",K
    optimal = drived + (measured-drived)*K
    last_optimal_convarance = (1-K)*P
    return optimal,last_optimal_convarance

def generate_data():
    x = np.linspace(0,50,num=500)
    y = 2*x + np.random.normal(loc=0.0,scale=0.5 ,size=x.shape)
    return x,y

def measure_data(data):
    measured = data + np.random.normal(loc=0.0, scale=1)
    return measured

x,y_real= generate_data()

y_measured = []
y_optimal = []

for data in y_real:
    y_measured.append(measure_data(data))

opt =0 
lst_var =100
for i,data in enumerate(y_measured):
    opt,lst_var= kalman_filter(data,opt,lst_var)
    y_optimal.append(opt)

plt.plot(x,y_real,'r')
plt.plot(x,y_optimal,'b')
plt.plot(x,y_measured,'pink')
plt.show()

我们看到，蓝色线较之粉色线，收敛了很多。写得很烂，好吧，大概就是这么个意思了，到这里，通俗理解卡尔曼滤波过程就结束了。。。