对Kalman Filter的一点简单的理解

本文简单介绍了卡尔曼滤波(Kalman Filter)的基本原理以及我对卡尔曼滤波的一些理解。

首先谈一下我的一点点理解

卡尔曼滤波是目前应用很广泛的一种滤波方法，最早由Kalman老先生在1960年提出，网上可以找到原文。这种方法最开始用在航天领域，作为轨道矫正的一种方法，有很好的效果。

卡尔曼滤波的方法的核心思想，就是用另一个测量空间的观测值去纠正当前空间对被测量的量的估计。简单来说，就是用一种方法去测量一个量。同时建立一个模型去估计这个测量的量，最后，按权重的方式求这两种方式的和，就是滤波之后的量的值。而这个权重的大小，就是卡尔曼系数。首先，我们假设要测量的量为x, 这个量有一个模型去描述其随时间的变化，例如计算每天的温度变化，可以大致根据之前几天的温度变化规律得到一个计算矩阵，这里也有一个计算模型去计算这个变量x

xt=Fxt−1+wt

wt∼N(0,Q)

其中F为转换矩阵，wt−1表示t−1时刻的噪声，且该噪声服从高斯分布。在其他的卡尔曼滤波公式推导中，会有一个额外的控制量，这里不考虑这个量。

对于测量矩阵，也有一个公式去转换。例如测量温度可以用温度传感器来测量，但是温度传感器的测量是因为温度改变了电阻的阻值，所以根据电压电流以及电阻随温度的变化曲线而计算出来的。在卡尔曼模型中，这一公式可以表示为如下等式

zt=Hxt+vt

vt∼N(0,R)

其中，zt是通过测量的量，对应到上述的例子中，就是温度传感器的电阻阻值，xt就是温度。H是测量矩阵，用来将测量的量转换成要估计的量。vt是测量过程中存在的误差。同样的，vt也是服从高斯分布的白噪声。

然后就是卡尔曼滤波的核心思想了，有了这两种方法得到的xt，那么怎么得到一个更准确的估计值。所以需要将两种方法得到的估计值进行算一下加权平均，就得到了最优的估计值。所以卡尔曼滤波的方法如下：

1. 首先根据模型计算当前时刻的估计值
   x′t=Fxt−1+wt

2. 然后根据测量矩阵计算当前的测量值的估计值

z′t=Hx′t+vt

1. 然后计算测量值和测量估计值之间的差，并以此作为对最终估计值的调整。从这里可以看出，如果x′t的很准，就是说此时zt的值和z′t=的值相差很小，那么zt对于xt的修正也就越少。但是如果估计值和测量值相差很大，那么ztt对xt的修正也就越大。其中，Kt是卡尔曼增益，表示滤波器对测量值的信任程度。

xt=x′t+Kt∗(zt−z′t)

那么如何估计卡尔曼增益，可以用贝叶斯估计的方法推导，也可以用最小二乘法的方式推导，这里用最小二乘法的方式推导

我们假设真实值是Xt，那么卡尔曼滤波计算得到的估计值和真实值之间的协方差

P(xt∣Xt)=E[(Xt−xt)(Xt−xt)T]

卡尔曼滤波的估计值和模型的估计值之间的协方差，用来评估两种估计的差别
P(x∣x′)=E[(xt−x′t)(xt−x′t)T]
根据卡尔曼的估计公式以及测量公式，可以得到
P(xt∣Xt)=E[(Xt−x′t−Kt∗(zt−z′t))(Xt−x′t−Kt∗(zt−z′t))T]=E[((I−KtH)(Xt−x′t)−Ktvt)((I−KtH)(Xt−x′t)−Ktvt)T]
把上述等式展开，可以得到
P(xt∣Xt)=(I−KtH)P(xt∣x′t)(I−KtH)+KtE[vtvTt]KTt=P(xt∣x′t)−KtHP(xt∣x′t)−P(xt∣x′t)HTKTt+Kt(HP(xt∣x′t)HT+R)KTt
所以，如果我们要估计的更准确，那么就要P(xt∣Xt)更小，就是说真实值和卡尔曼滤波的估计值之间的协方差最小。不考虑估计值之间的相关，那么协方差矩阵的对角线元素就表示了卡尔曼估计值和真实值之间的方差。接下来就是求方差最小的情况下对应的卡尔曼增益Kt。可以用矩阵的迹的方法求解
tr(P(xt∣Xt))=tr(P(xt∣x′t))−2tr(KtHP(xt∣x′t))+tr(Kt(HP(xt∣x′t)HT+R)KTt)
可以看出，tr(P(xt∣Xt))是Kt的二次函数，所以根据二次函数求极值的方法，对tr(P(x_t|X_t))求导，得到

d(tr(P(xt∣Xt)))d(Kt)=−2(HP(xt∣x′t))T+2Kt(HP(xt∣x′t)HT+R)

令d(tr(P(xt∣Xt)))d(Kt)=0，所以有
Kt=P(xt∣x′t)HT(HP(xt∣x′t)HT+R)−1
把Kt的结果带入到P(xt∣Xt)的表达式中，有
P(xt∣Xt)=P(xt∣x′t)−KtHP(xt∣x′t)−P(xt∣x′t)HTKTt+Kt(HP(xt∣x′t)HT+R)KTt=P(xt∣x′t)−KtHP(xt∣x′t)−HP(xt∣x′t)TP(xt∣x′t)HTHP(xt∣x′t)HT+R+HP(xt∣x′t)TP(xt∣x′t)HTHP(xt∣x′t)HT+R=P(xt∣x′t)−KtHP(xt∣x′t)=(I−KtH)P(xt∣x′t)
所以根据上述的推导计算，可以得到卡尔曼滤波的计算过程：

1. 首先，根据已知的模型，以及上一时刻的卡尔曼估计值，计算当前时刻的模型预测值

x′t=Fxt−1

1. 根据当前的模型预测值，计算对应的协方差

P(xt∣x′t)=FP(xt∣Xt)FT

1. 根据当前的协方差和测量空间的转换矩阵，计算当前时刻的卡尔曼增益

Kt=P(xt∣x′t)HT(HP(xt∣x′t)HT+R)−1

1. 根据卡尔曼增益和测量值，计算当前时刻的卡尔曼估计值

xt=x′t+Kt(zt−Hx′t)

1. 计算了当前时刻的卡尔曼估计值之后，还需要计算当前的估计值和真实值的协方差矩阵，方便下一次计算

P(xt∣Xt)=(I−HKt)P(xt∣x′t)

以上就是卡尔曼滤波的基本过程，以及一些简单的推导。总体上说理解卡尔曼滤波应该算一种最优估计的算法。也是应用很广泛的，然后卡尔曼滤波的推导方法也有很多，除了最小二乘法，也可以从贝叶斯估计的角度推导。两者是类似的。

Code