卡尔曼滤波

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

$$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)\)

然后我这里给出相应的滤波过程:

  1. 预测\begin{equation}\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\end{equation}
  2. 更新\begin{equation}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\end{equation}

这里也有几点说明。变量上面带一横的表示是先验值,变量上面带一个尖帽子的表示后验值,也就是待求的最优值。\(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()

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

发表评论

电子邮件地址不会被公开。 必填项已用*标注