本文简单介绍了卡尔曼滤波(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)(Xt−xt)T]
卡尔曼滤波的估计值和模型的估计值之间的协方差，用来评估两种估计的差别
P
(
x
∣
x
′
)
=
E
[
(
x
t
−
x
t
′
)
(
x
t
−
x
t
′
)
T
]
P(x|x')=E[(x_t-x_t')(x_t-x_t')^T]
P(x∣x′)=E[(xt−xt′)(xt−xt′)T]
根据卡尔曼的估计公式以及测量公式，可以得到
P
(
x
t
∣
X
t
)
=
E
[
(
X
t
−
x
t
′
−
K
t
∗
(
z
t
−
z
t
′
)
)
(
X
t
−
x
t
′
−
K
t
∗
(
z
t
−
z
t
′
)
)
T
]
=
E
[
(
(
I
−
K
t
H
)
(
X
t
−
x
t
′
)
−
K
t
v
t
)
(
(
I
−
K
t
H
)
(
X
t
−
x
t
′
)
−
K
t
v
t
)
T
]
P(x_t|X_t)=E[(X_t-x_t'-K_t*(z_t-z_t'))(X_t-x_t'-K_t*(z_t-z_t'))^T] \\ =E[((I-K_tH)(X_t-x_t')-K_tv_t)((I-K_tH)(X_t-x_t')-K_tv_t)^T]
P(xt∣Xt)=E[(Xt−xt′−Kt∗(zt−zt′))(Xt−xt′−Kt∗(zt−zt′))T]=E[((I−KtH)(Xt−xt′)−Ktvt)((I−KtH)(Xt−xt′)−Ktvt)T]
把上述等式展开，可以得到
P
(
x
t
∣
X
t
)
=
(
I
−
K
t
H
)
P
(
x
t
∣
x
t
′
)
(
I
−
K
t
H
)
+
K
t
E
[
v
t
v
t
T
]
K
t
T
=
P
(
x
t
∣
x
t
′
)
−
K
t
H
P
(
x
t
∣
x
t
′
)
−
P
(
x
t
∣
x
t
′
)
H
T
K
t
T
+
K
t
(
H
P
(
x
t
∣
x
t
′
)
H
T
+
R
)
K
t
T
P(x_t|X_t)=(I-K_tH)P(x_t|x_t')(I-K_tH)+K_tE[v_tv_t^T]K_t^T \\ =P(x_t|x_t')-K_tHP(x_t|x_t')-P(x_t|x_t')H^TK_t^T+K_t(HP(x_t|x_t')H^T+R)K_t^T
P(xt∣Xt)=(I−KtH)P(xt∣xt′)(I−KtH)+KtE[vtvtT]KtT=P(xt∣xt′)−KtHP(xt∣xt′)−P(xt∣xt′)HTKtT+Kt(HP(xt∣xt′)HT+R)KtT
所以，如果我们要估计的更准确，那么就要
P
(
x
t
∣
X
t
)
P(x_t|X_t)
P(xt∣Xt)更小，就是说真实值和卡尔曼滤波的估计值之间的协方差最小。不考虑估计值之间的相关，那么协方差矩阵的对角线元素就表示了卡尔曼估计值和真实值之间的方差。接下来就是求方差最小的情况下对应的卡尔曼增益
K
t
K_t
Kt。可以用矩阵的迹的方法求解
t
r
(
P
(
x
t
∣
X
t
)
)
=
t
r
(
P
(
x
t
∣
x
t
′
)
)
−
2
t
r
(
K
t
H
P
(
x
t
∣
x
t
′
)
)
+
t
r
(
K
t
(
H
P
(
x
t
∣
x
t
′
)
H
T
+
R
)
K
t
T
)
tr(P(x_t|X_t)) = tr(P(x_t|x_t'))-2tr(K_tHP(x_t|x_t'))+tr(K_t(HP(x_t|x_t')H^T+R)K_t^T)
tr(P(xt∣Xt))=tr(P(xt∣xt′))−2tr(KtHP(xt∣xt′))+tr(Kt(HP(xt∣xt′)HT+R)KtT)
可以看出，
t
r
(
P
(
x
t
∣
X
t
)
)
tr(P(x_t|X_t))
tr(P(xt∣Xt))是
K
t
K_t
Kt的二次函数，所以根据二次函数求极值的方法，对tr(P(x_t|X_t))求导，得到
d
(
t
r
(
P
(
x
t
∣
X
t
)
)
)
d
(
K
t
)
=
−
2
(
H
P
(
x
t
∣
x
t
′
)
)
T
+
2
K
t
(
H
P
(
x
t
∣
x
t
′
)
H
T
+
R
)
\frac{d(tr(P(x_t|X_t)))}{d(K_t)}=-2(HP(x_t|x_t'))^T+2K_t(HP(x_t|x_t')H^T+R)
d(Kt)d(tr(P(xt∣Xt)))=−2(HP(xt∣xt′))T+2Kt(HP(xt∣xt′)HT+R)
令
d
(
t
r
(
P
(
x
t
∣
X
t
)
)
)
d
(
K
t
)
=
0
\frac{d(tr(P(x_t|X_t)))}{d(K_t)}=0
d(Kt)d(tr(P(xt∣Xt)))=0，所以有
K
t
=
P
(
x
t
∣
x
t
′
)
H
T
(
H
P
(
x
t
∣
x
t
′
)
H
T
+
R
)
−
1
K_t=P(x_t|x_t')H^T(HP(x_t|x_t')H^T+R)^{-1}
Kt=P(xt∣xt′)HT(HP(xt∣xt′)HT+R)−1
把
K
t
K_t
Kt的结果带入到
P
(
x
t
∣
X
t
)
P(x_t|X_t)
P(xt∣Xt)的表达式中，有
P
(
x
t
∣
X
t
)
=
P
(
x
t
∣
x
t
′
)
−
K
t
H
P
(
x
t
∣
x
t
′
)
−
P
(
x
t
∣
x
t
′
)
H
T
K
t
T
+
K
t
(
H
P
(
x
t
∣
x
t
′
)
H
T
+
R
)
K
t
T
=
P
(
x
t
∣
x
t
′
)
−
K
t
H
P
(
x
t
∣
x
t
′
)
−
H
P
(
x
t
∣
x
t
′
)
T
P
(
x
t
∣
x
t
′
)
H
T
H
P
(
x
t
∣
x
t
′
)
H
T
+
R
+
H
P
(
x
t
∣
x
t
′
)
T
P
(
x
t
∣
x
t
′
)
H
T
H
P
(
x
t
∣
x
t
′
)
H
T
+
R
=
P
(
x
t
∣
x
t
′
)
−
K
t
H
P
(
x
t
∣
x
t
′
)
=
(
I
−
K
t
H
)
P
(
x
t
∣
x
t
′
)
P(x_t|X_t)=P(x_t|x_t')-K_tHP(x_t|x_t')-P(x_t|x_t')H^TK_t^T+K_t(HP(x_t|x_t')H^T+R)K_t^T\\ =P(x_t|x_t')-K_tHP(x_t|x_t')-\frac{HP(x_t|x_t')^TP(x_t|x_t')H^T}{HP(x_t|x_t')H^T+R}+\frac{HP(x_t|x_t')^TP(x_t|x_t')H^T}{HP(x_t|x_t')H^T+R}\\ =P(x_t|x_t')-K_tHP(x_t|x_t')\\ =(I-K_tH)P(x_t|x_t')
P(xt∣Xt)=P(xt∣xt′)−KtHP(xt∣xt′)−P(xt∣xt′)HTKtT+Kt(HP(xt∣xt′)HT+R)KtT=P(xt∣xt′)−KtHP(xt∣xt′)−HP(xt∣xt′)HT+RHP(xt∣xt′)TP(xt∣xt′)HT+HP(xt∣xt′)HT+RHP(xt∣xt′)TP(xt∣xt′)HT=P(xt∣xt′)−KtHP(xt∣xt′)=(I−KtH)P(xt∣xt′)
计算过程
所以根据上述的推导计算，可以得到卡尔曼滤波的计算过程：
- 根据给定的训练集数据 D = { X , Y } D=\{X, Y\} D={X,Y}，其中 X = { x 1 , x 2 , . . . , x n } X=\{x_1, x_2, ..., x_n\} X={x1,x2,...,xn}， Y = { y 1 , y 2 , . . . , y n } Y=\{y_1, y_2, ..., y_n\} Y={y1,y2,...,yn}，计算状态矩阵 F F F和测量矩阵 H H H，以及误差的分布方差 Q Q Q和 R R R。
- 根据已知的模型，以及上一时刻的卡尔曼估计值，计算当前时刻的模型预测值
x t ′ = F x t − 1 x_t'=Fx_{t-1} xt′=Fxt−1
- 根据当前的模型预测值，计算对应的协方差
P ( x t ∣ x t ′ ) = F P ( x t ∣ X ) F T + Q P(x_t|x_t')=FP(x_t|X)F^T + Q P(xt∣xt′)=FP(xt∣X)FT+Q
- 根据当前的协方差和测量空间的转换矩阵，计算当前时刻的卡尔曼增益
K t = P ( x t ∣ x t ′ ) H T ( H P ( x t ∣ x t ′ ) H T + R ) − 1 K_t=P(x_t|x_t')H^T(HP(x_t|x_t')H^T+R)^{-1} Kt=P(xt∣xt′)HT(HP(xt∣xt′)HT+R)−1
- 根据卡尔曼增益和测量值，计算当前时刻的卡尔曼估计值
x t = x t ′ + K t ( z t − H x t ′ ) x_t=x_t'+K_t(z_t-Hx_t') xt=xt′+Kt(zt−Hxt′)
- 计算了当前时刻的卡尔曼估计值之后，还需要计算当前的估计值和真实值的协方差矩阵，方便下一次计算
P ( x t ∣ X t ) = ( I − H K t ) P ( x t ∣ x t ′ ) P(x_t|X_t)=(I-HK_t)P(x_t|x_t') P(xt∣Xt)=(I−HKt)P(xt∣xt′)
以上就是卡尔曼滤波的基本过程，以及一些简单的推导。总体上说理解卡尔曼滤波应该算一种最优估计的算法。也是应用很广泛的，然后卡尔曼滤波的推导方法也有很多，除了最小二乘法，也可以从贝叶斯估计的角度推导。两者是类似的。
3万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
