本文简单介绍了卡尔曼滤波(Kalman Filter)的基本原理。
基本原理
卡尔曼滤波是目前应用很广泛的一种滤波方法,最早由Kalman老先生在1960年提出,网上可以找到原文。这种方法最开始用在航天领域,作为轨道矫正的一种方法,有很好的效果。
卡尔曼滤波的方法的核心思想,就是用另一个测量空间的观测值去纠正当前空间对被测量的量的估计。简单来说,就是用一种方法去测量一个量。同时建立一个模型去估计这个测量的量,最后,按权重的方式求这两种方式的和,就是滤波之后的量的值。而这个权重的大小,就是卡尔曼系数。
公式推导
首先,我们假设要测量的量为 x x x, 这个量有一个模型去描述其随时间的变化,例如计算每天的温度变化,可以大致根据之前几天的温度变化规律得到一个计算矩阵,这里也有一个计算模型去计算这个变量 x x x
x t = F x t − 1 + w t x_t=Fx_{t-1} + w_t xt=Fxt−1+wt
w t ∼ N ( 0 , Q ) w_t \sim N(0,Q) wt∼N(0,Q)
其中 F F F为转换矩阵, w t − 1 w_{t-1} wt−1表示 t − 1 t-1 t−1时刻的噪声,且该噪声服从高斯分布。在其他的卡尔曼滤波公式推导中,会有一个额外的控制量,这里不考虑这个量。
对于测量矩阵,也有一个公式去转换。例如测量温度可以用温度传感器来测量,但是温度传感器的测量是因为温度改变了电阻的阻值,所以根据电压电流以及电阻随温度的变化曲线而计算出来的。在卡尔曼模型中,这一公式可以表示为如下等式
z t = H x t + v t z_t=Hx_t+v_t zt=Hxt+vt
v t ∼ N ( 0 , R ) v_t \sim N(0,R) vt∼N(0,R)
其中, z t z_t zt是通过测量的量,对应到上述的例子中,就是温度传感器的电阻阻值, x t x_t xt就是温度。 H H H是测量矩阵,用来将测量的量转换成要估计的量。 v t v_t vt是测量过程中存在的误差。同样的, v t v_t vt也是服从高斯分布的白噪声。
然后就是卡尔曼滤波的核心思想了,有了这两种方法得到的 x t x_t xt,那么怎么得到一个更准确的估计值。所以需要将两种方法得到的估计值进行算一下加权平均,就得到了最优的估计值。所以卡尔曼滤波的方法如下:
-
首先根据模型计算当前时刻的估计值
x t ′ = F x t − 1 + w t x_t'=Fx_{t-1} + w_t xt′=Fxt−1+wt -
然后根据测量矩阵计算当前的测量值的估计值
z t ′ = H x t ′ + v t z_t'=Hx_t'+v_t zt′=Hxt′+vt
- 然后计算测量值和测量估计值之间的差,并以此作为对最终估计值的调整。从这里可以看出,如果 x t ′ x_t' xt′估计的很准,就是说此时 z t z_t zt的值和 z t ′ z_t' zt′的值相差很小,那么 z t z_t zt对于 x t x_t xt的修正也就越少。但是如果估计值和测量值相差很大,那么 z t z_t zt对 x t x_t xt的修正也就越大。其中, K t K_t Kt是卡尔曼增益,表示滤波器对测量值的信任程度。
x t = x t ′ + K t ∗ ( z t − z t ′ ) x_t=x_t'+K_t*(z_t-z_t') xt=xt′+Kt∗(zt−zt′)
那么如何估计卡尔曼增益,可以用贝叶斯估计的方法推导,也可以用最小二乘法的方式推导,这里用最小二乘法的方式推导
我们假设真实值是 X t X_t Xt,那么卡尔曼滤波计算得到的估计值和真实值之间的协方差
P ( x t ∣ X t ) = E [ ( X t − x t ) ( X t − x t ) T ] P(x_t|X_t)= E[(X_t-x_t)(X_t-x_t)^T] P(xt∣Xt)=E[(Xt−xt)(X