Kalman Filter
作者：互联网
Note: 本文基本为参考资料1中视频的笔记版本。
引言：系统描述
考虑如下离散系统
{
x
k
=
A
x
k
−
1
+
B
u
k
−
1
+
w
k
−
1
z
k
=
H
x
k
+
v
k
(1-1)
\begin{cases} x_k = Ax_{k-1} + Bu_{k-1} + w_{k-1} \\ z_k = Hx_k + v_k \tag{1-1} \end{cases}
{xk=Axk−1+Buk−1+wk−1zk=Hxk+vk(1-1)
其中
w
k
w_{k}
wk为过程噪声，满足期望为0，方差为协方差Q的正态分布；
v
k
v_k
vk为测量噪声，满足期望为0，方差为协方差R的正态分布；即：
{
W
∼
N
(
0
,
Q
)
V
∼
N
(
0
,
R
)
(1-2)
\begin{cases} W∼N(0, Q) \\ V∼N(0, R) \end{cases} \tag{1-2}
{W∼N(0,Q)V∼N(0,R)(1-2)
W
W
W与
V
V
V均为矢量，且有：
{
Q
=
E
(
w
w
T
)
R
=
E
(
v
v
T
)
(1-3)
\begin{cases} Q = E(ww^T) \\ R = E(vv^T) \end{cases} \tag{1-3}
{Q=E(wwT)R=E(vvT)(1-3)
式(1-3)可由
D
(
X
)
=
E
(
X
2
)
−
(
E
(
X
)
)
2
D(X) = E(X^2) - (E(X))^2
D(X)=E(X2)−(E(X))2导出，其中
E
(
X
)
表
示
为
随
机
变
量
X
的
期
望
E(X)表示为随机变量X的期望
E(X)表示为随机变量X的期望，
D
(
X
)
D(X)
D(X)表示为随机变量X的方差。
引言：测量数据估计
以一个简单的例子说明如何做测量数据估计。假设某测量系统测量出来的k次信号为
z
1
,
z
2
,
.
.
.
z
k
z_1, z_2,...z_k
z1,z2,...zk，那么很自然的思路是求取其平均值作为估计值，即估计值
x
k
^
=
1
k
∑
i
=
1
k
z
i
\hat{x_k} = \frac{1}{k}\sum_{i=1}^{k}{z_i}
xk^=k1∑i=1kzi。显然，当k越大，则说明
x
k
^
\hat{x_k}
xk^越接近真值。然而该方式需要多次采集数据才能得到单次结果，不利于实时性应用，因此需要一个递推算法。具体方法可由下式导出：
x
k
^
=
1
k
(
z
1
+
z
2
+
.
.
.
+
z
k
)
=
1
k
(
∑
i
=
1
k
−
1
z
i
+
z
k
)
=
k
−
1
k
(
1
k
−
1
∑
i
=
1
k
−
1
z
i
+
1
k
−
1
z
k
)
=
k
−
1
k
(
x
k
−
1
^
+
1
k
−
1
z
k
)
=
k
−
1
k
x
k
−
1
^
+
1
k
z
k
=
x
k
−
1
^
+
1
k
(
z
k
−
x
k
−
1
^
)
(2-1)
\hat{x_k} = \frac{1}{k}(z_1 + z_2 + ... + z_k) \\ = \frac{1}{k}(\sum_{i=1}^{k-1}z_i + z_k) \\ = \frac{k-1}{k}(\frac{1}{k-1}\sum_{i=1}^{k-1}z_i + \frac{1}{k-1}z_k) \\ = \frac{k-1}{k}(\hat{x_{k-1}} + \frac{1}{k-1}z_k) \\ = \frac{k-1}{k}\hat{x_{k-1}} + \frac{1}{k}z_k \\ = \hat{x_{k-1}} + \frac{1}{k}(z_k - \hat{x_{k-1}}) \tag{2-1}
xk^=k1(z1+z2+...+zk)=k1(i=1∑k−1zi+zk)=kk−1(k−11i=1∑k−1zi+k−11zk)=kk−1(xk−1^+k−11zk)=kk−1xk−1^+k1zk=xk−1^+k1(zk−xk−1^)(2-1)
从式(2-1)可看出，最新一次的估计值为上一次的估计值加上一个系数乘以最新测量值与上一次估计值之差，此即为测量数据估计的递推式，同样也是Kalman Filter的基本思路。
Kalman Filter的一种推导过程
本文仅仅以基于方差最小估计的角度来推导Kalman Filter，实际上推导它的方式还有很多种，角度不同都可以得出结论。
Kalman Gain的引入
Kalman Filter本质是一种基于方差最小的估计方法，通过寻找估计误差的方差最小值来进行估计。对于式(1-1)所描述的离散系统，有两种方式可以获得系统状态估计值，即：
{
x
k
−
^
=
A
x
k
−
1
^
+
B
u
k
−
1
x
k
_
m
e
a
s
=
H
−
1
z
k
(4-1)
\begin{cases} \hat{x_k^-} = A\hat{x_{k-1}} + Bu_{k-1} \\ x_{k\_meas} = H^{-1}z_k \tag{4-1} \end{cases}
{xk−^=Axk−1^+Buk−1xk_meas=H−1zk(4-1)
前者
x
k
−
^
\hat{x_k^-}
xk−^为先验估计值，后者
x
k
_
m
e
a
s
x_{k\_meas}
xk_meas则为测量估计值。为了得到当前第k次的最优估计值，可以利用式(2-1)的思路：
x
k
^
=
x
k
−
^
+
K
1
k
(
H
−
1
z
k
−
x
k
−
^
)
=
x
k
−
^
+
K
k
(
z
k
−
H
x
k
−
^
)
(4-2)
\hat{x_k} = \hat{x_k^-} + K_{1k}(H^{-1}z_k - \hat{x_k^-}) \\ = \hat{x_k^-} + K_{k}(z_k - H\hat{x_k^-}) \tag{4-2}
xk^=xk−^+K1k(H−1zk−xk−^)=xk−^+Kk(zk−Hxk−^)(4-2)
其中
K
k
K_{k}
Kk即为Kalman Gain。
Note: Kalman Filter作为一种观测器，其基本估计表达式为式(4-2)，与隆贝格观测器类似，同样的需要满足系统能观性
Kalman Gain的求取
为了使估计误差
e
k
=
x
k
−
x
k
^
e_k = x_k - \hat{x_k}
ek=xk−xk^达到最小，即使
e
k
e_k
ek的协方差
p
k
p_k
pk的主对角线求和最小（表示每一个状态
e
k
(
i
)
e_k(i)
ek(i)的方差都为最小值），用数学语言表达如下：
min
e
k
=
x
k
−
x
k
^
⇒
min
t
r
(
p
k
)
(4-3)
\min e_k = x_k - \hat{x_k} \\ \Rightarrow \min tr(p_k) \tag{4-3}
minek=xk−xk^⇒mintr(pk)(4-3)
根据
D
(
X
)
=
E
(
X
2
)
−
(
E
(
X
)
)
2
D(X) = E(X^2) - (E(X))^2
D(X)=E(X2)−(E(X))2可得出：
p
k
=
E
(
e
k
e
k
T
)
(4-4)
p_k = E(e_ke_k^T) \tag{4-4}
pk=E(ekekT)(4-4)
将式(1-1)的测量值
z
k
z_k
zk计算式，式(4-2)带入
e
k
=
x
k
−
x
k
^
e_k = x_k - \hat{x_k}
ek=xk−xk^可得：
e
k
=
(
I
−
K
k
H
)
(
x
k
−
x
k
^
)
−
K
k
v
k
=
(
I
−
K
k
H
)
e
k
−
−
K
k
v
k
(4-5)
e_k = (I - K_kH)(x_k - \hat{x_k}) - K_kv_k \\ = (I - K_kH)e_k^- - K_kv_k \tag{4-5}
ek=(I−KkH)(xk−xk^)−Kkvk=(I−KkH)ek−−Kkvk(4-5)
其中
e
k
−
e_k^-
ek−表示先验估计误差。因此式(4-4)可导出为：
p
k
=
E
(
e
k
e
k
T
)
=
E
(
(
I
−
K
k
H
)
e
k
−
e
k
−
T
(
I
−
H
T
K
k
T
)
−
(
I
−
K
k
H
)
e
k
−
v
k
T
K
k
T
−
.
.
.
K
k
v
k
e
k
−
T
(
I
−
H
T
K
k
T
)
+
K
k
v
k
v
k
T
K
k
T
)
=
(
I
−
K
k
H
)
E
(
e
k
−
e
k
−
T
)
(
I
−
H
T
K
k
T
)
−
(
I
−
K
k
H
)
E
(
e
k
−
v
k
T
)
K
k
T
−
.
.
.
K
k
E
(
v
k
e
k
−
T
)
(
I
−
H
T
K
k
T
)
+
K
k
E
(
v
k
v
k
T
)
K
k
T
(4-6)
p_k = E(e_ke_k^T) \\ = E((I-K_kH)e_k^-e_k^{-T}(I-H^TK_k^T) - (I-K_kH)e_k^-v_k^TK_k^T - ... \\ K_kv_ke_k^{-T}(I-H^TK_k^T)+K_kv_kv_k^TK_k^T) \\ = (I-K_kH)E(e_k^-e_k^{-T})(I-H^TK_k^T) - (I-K_kH)E(e_k^-v_k^T)K_k^T - ... \\ K_kE(v_ke_k^{-T})(I-H^TK_k^T) + K_kE(v_kv_k^T)K_k^T \tag{4-6}
pk=E(ekekT)=E((I−KkH)ek−ek−T(I−HTKkT)−(I−KkH)ek−vkTKkT−...Kkvkek−T(I−HTKkT)+KkvkvkTKkT)=(I−KkH)E(ek−ek−T)(I−HTKkT)−(I−KkH)E(ek−vkT)KkT−...KkE(vkek−T)(I−HTKkT)+KkE(vkvkT)KkT(4-6)
E
(
e
k
−
e
k
−
T
)
E(e_k^-e_k^{-T})
E(ek−ek−T)即为先验估计误差协方差
p
k
−
p_k^-
pk−，
E
(
v
k
v
k
T
)
E(v_kv_k^T)
E(vkvkT)即为R。由于第k次的先验估计误差
e
k
−
e_k^-
ek−与测量误差
v
k
T
v_k^T
vkT不相关，因此
E
(
e
k
−
v
k
T
)
=
E
(
e
k
−
)
E
(
v
k
T
)
=
0
E(e_k^-v_k^T) = E(e_k^-)E(v_k^T)=0
E(ek−vkT)=E(ek−)E(vkT)=0，
E
(
v
k
e
k
−
T
)
=
E
(
v
k
)
E
(
e
k
−
T
)
=
0
E(v_ke_k^{-T})=E(v_k)E(e_k^{-T})=0
E(vkek−T)=E(vk)E(ek−T)=0。故式(4-6)可化简为：
p
k
=
E
(
e
k
e
k
T
)
=
(
I
−
K
k
H
)
p
k
−
(
I
−
H
T
K
k
T
)
+
K
k
R
K
k
T
=
p
k
−
−
p
k
−
H
T
K
k
T
−
K
k
H
p
k
−
+
K
k
H
p
k
−
H
T
K
k
T
+
K
k
R
K
k
T
(4-7)
p_k = E(e_ke_k^T) \\ = (I-K_kH)p_k^-(I-H^TK_k^T) + K_kRK_k^T \\ = p_k^- - p_k^-H^TK_k^T - K_kHp_k^- + K_kHp_k^-H^TK_k^T + K_kRK_k^T \tag{4-7}
pk=E(ekekT)=(I−KkH)pk−(I−HTKkT)+KkRKkT=pk−−pk−HTKkT−KkHpk−+KkHpk−HTKkT+KkRKkT(4-7)
为了求解问题(4-3)，只需要(4-3)对
K
k
K_k
Kk求导并令其为0即可，故：
d
(
t
r
(
p
k
)
)
=
−
d
(
t
r
(
p
k
−
H
T
K
k
T
+
K
k
H
p
k
−
)
+
d
(
t
r
(
K
k
H
p
k
−
H
T
K
k
T
)
+
d
(
t
r
(
(
K
k
R
K
k
T
)
)
=
−
2
d
(
t
r
(
K
k
H
p
k
−
)
)
+
d
(
t
r
(
K
k
H
p
k
−
H
T
K
k
T
)
+
d
(
t
r
(
(
K
k
R
K
k
T
)
)
=
t
r
(
(
−
2
p
k
−
T
H
T
+
2
K
k
R
T
+
2
K
k
H
p
k
−
T
H
T
)
d
K
k
)
(4-8)
d(tr(p_k)) = - d(tr(p_k^-H^TK_k^T + K_kHp_k^-) + d(tr(K_kHp_k^-H^TK_k^T) + d(tr((K_kRK_k^T)) \\ = -2d(tr(K_kHp_k^-)) + d(tr(K_kHp_k^-H^TK_k^T) + d(tr((K_kRK_k^T)) \\ = tr((-2p_k^{-T}H^T+2K_kR^T+2K_kHp_k^{-T}H^T)dK_k) \tag{4-8}
d(tr(pk))=−d(tr(pk−HTKkT+KkHpk−)+d(tr(KkHpk−HTKkT)+d(tr((KkRKkT))=−2d(tr(KkHpk−))+d(tr(KkHpk−HTKkT)+d(tr((KkRKkT))=tr((−2pk−THT+2KkRT+2KkHpk−THT)dKk)(4-8)
又协方差为对称阵，故
R
T
=
R
R^T = R
RT=R，
p
k
−
T
=
p
k
−
p_k^{-T} = p_k^-
pk−T=pk−，因此有：
d
(
t
r
(
p
k
)
)
d
K
k
=
−
2
p
k
−
H
T
+
2
K
k
R
+
2
K
k
H
p
k
−
H
T
=
0
(4-9)
\frac{d(tr(p_k))}{dK_k} = -2p_k^{-}H^T+2K_kR+2K_kHp_k^{-}H^T = 0 \tag{4-9}
dKkd(tr(pk))=−2pk−HT+2KkR+2KkHpk−HT=0(4-9)
即：
K
k
=
p
k
−
H
T
(
R
+
H
p
k
−
H
T
)
−
1
(4-10)
K_k = p_k^-H^T(R+Hp_k^-H^T)^{-1} \tag{4-10}
Kk=pk−HT(R+Hpk−HT)−1(4-10)
先验估计误差协方差 p k − p_k^- pk−的求取
求取Kalman Gain时，需要使用到先验估计误差协方差
p
k
−
p_k^-
pk−，由式(4-6)可知，其表达式如下：
p
k
−
=
E
(
e
k
−
e
k
−
T
)
(5-1)
p_k^- = E(e_k^-e_k^{-T}) \tag{5-1}
pk−=E(ek−ek−T)(5-1)
将离散系统(1-1)中的状态表达式，式(4-2)带入到
e
k
−
=
x
k
−
x
k
^
e_k^-=x_k - \hat{x_k}
ek−=xk−xk^中，可得到：
e
k
−
=
A
(
x
k
−
1
−
x
k
−
1
^
)
+
w
k
−
1
=
A
e
k
−
1
+
w
k
−
1
(5-2)
e_k^- = A(x_{k-1} - \hat{x_{k-1}}) + w_{k-1} \\ = Ae_{k-1} + w_{k-1} \tag{5-2}
ek−=A(xk−1−xk−1^)+wk−1=Aek−1+wk−1(5-2)
将式(5-2)带入式(5-1)可得到：
p
k
−
=
E
(
e
k
−
e
k
−
T
)
=
A
E
(
e
k
−
1
e
k
−
1
T
)
A
T
+
A
E
(
e
k
−
1
w
k
−
1
T
)
+
E
(
w
k
−
1
e
k
−
1
T
)
A
T
+
E
(
w
k
−
1
w
k
−
1
T
)
(5-3)
p_k^- = E(e_k^-e_k^{-T}) \\ = AE(e_{k-1}e_{k-1}^T)A^T + AE(e_{k-1}w_{k-1}^T) + E(w_{k-1}e_{k-1}^T)A^T + E(w_{k-1}w_{k-1}^T) \tag{5-3}
pk−=E(ek−ek−T)=AE(ek−1ek−1T)AT+AE(ek−1wk−1T)+E(wk−1ek−1T)AT+E(wk−1wk−1T)(5-3)
E
(
e
k
−
1
e
k
−
1
T
)
E(e_{k-1}e_{k-1}^T)
E(ek−1ek−1T)即为上一次的估计误差协方差
p
k
−
1
p_{k-1}
pk−1，
E
(
w
k
−
1
w
k
−
1
T
)
E(w_{k-1}w_{k-1}^T)
E(wk−1wk−1T)即为Q。由于
e
k
−
1
e_{k-1}
ek−1仅与
w
k
−
2
w_{k-2}
wk−2相关，与
w
k
−
1
w_{k-1}
wk−1不相关，因此有：
{
E
(
e
k
−
1
w
k
−
1
T
)
=
E
(
e
k
−
1
)
E
(
w
k
−
1
T
)
=
0
E
(
w
k
−
1
e
k
−
1
T
)
=
E
(
w
k
−
1
)
E
(
e
k
−
1
T
)
=
0
(5-4)
\begin{cases} E(e_{k-1}w_{k-1}^T) = E(e_{k-1})E(w_{k-1}^T) = 0 \\ E(w_{k-1}e_{k-1}^T) = E(w_{k-1})E(e_{k-1}^T) = 0 \tag{5-4} \end{cases}
{E(ek−1wk−1T)=E(ek−1)E(wk−1T)=0E(wk−1ek−1T)=E(wk−1)E(ek−1T)=0(5-4)
故式(5-3)可化简为：
p
k
−
=
E
(
e
k
−
e
k
−
T
)
=
A
p
k
−
1
A
T
+
Q
(5-5)
p_k^- = E(e_k^-e_k^{-T}) \\ = Ap_{k-1}A^T + Q \tag{5-5}
pk−=E(ek−ek−T)=Apk−1AT+Q(5-5)
估计误差协方差 p k p_k pk的计算
由式(4-7)可知，估计误差协方差
p
k
p_k
pk的计算方式如下：
p
k
=
E
(
e
k
e
k
T
)
=
p
k
−
−
p
k
−
H
T
K
k
T
−
K
k
H
p
k
−
+
K
k
(
H
p
k
−
H
T
+
R
)
K
k
T
(6-1)
p_k = E(e_ke_k^T) \\ = p_k^- - p_k^-H^TK_k^T - K_kHp_k^- + K_k(Hp_k^-H^T + R)K_k^T \tag{6-1}
pk=E(ekekT)=pk−−pk−HTKkT−KkHpk−+Kk(Hpk−HT+R)KkT(6-1)
将式(4-10)带入式(6-1)可得：
p
k
=
E
(
e
k
e
k
T
)
=
p
k
−
−
p
k
−
H
T
K
k
T
−
K
k
H
p
k
−
+
p
k
−
H
T
(
R
+
H
p
k
−
H
T
)
−
1
(
H
p
k
−
H
T
+
R
)
K
k
T
=
p
k
−
−
K
k
H
p
k
−
=
(
I
−
K
k
H
)
p
k
−
(6-2)
p_k = E(e_ke_k^T) \\ = p_k^- - p_k^-H^TK_k^T - K_kHp_k^- + p_k^-H^T(R+Hp_k^-H^T)^{-1}(Hp_k^-H^T + R)K_k^T \\ = p_k^- - K_kHp_k^- \\ = (I - K_kH)p_k^- \tag{6-2}
pk=E(ekekT)=pk−−pk−HTKkT−KkHpk−+pk−HT(R+Hpk−HT)−1(Hpk−HT+R)KkT=pk−−KkHpk−=(I−KkH)pk−(6-2)
至此，Kalman Filter推导完毕。
总结
式(4-1)，式(4-2)，式(4-10)，式(5-5)，式(6-2)即为完整的Kalman Filter的计算过程，分为预测和校正两部分。预测部分为式(4-1)和式(5-5)，即：
{
先
验
：
x
k
−
^
=
A
x
k
−
1
^
+
B
u
k
−
1
先
验
误
差
协
方
差
：
p
k
−
=
A
p
k
−
1
A
T
+
Q
(7-1)
\begin{cases} 先验：\hat{x_k^-} = A\hat{x_{k-1}} + Bu_{k-1} \\ 先验误差协方差：p_k^- = Ap_{k-1}A^T + Q \tag{7-1} \end{cases}
{先验：xk−^=Axk−1^+Buk−1先验误差协方差：pk−=Apk−1AT+Q(7-1)
校正部分为式(4-2)，式(4-10)和式(6-2)，即：
{
卡
尔
曼
增
益
：
K
k
=
p
k
−
H
T
(
R
+
H
p
k
−
H
T
)
−
1
后
验
估
计
：
x
k
^
=
x
k
−
^
+
K
k
(
z
k
−
H
x
k
−
^
)
误
差
协
方
差
：
p
k
=
(
I
−
K
k
H
)
p
k
−
(7-2)
\begin{cases} 卡尔曼增益：K_k = p_k^-H^T(R+Hp_k^-H^T)^{-1} \\ 后验估计：\hat{x_k} = \hat{x_k^-} + K_{k}(z_k - H\hat{x_k^-}) \\ 误差协方差：p_k = (I - K_kH)p_k^- \tag{7-2} \end{cases}
⎩⎪⎨⎪⎧卡尔曼增益：Kk=pk−HT(R+Hpk−HT)−1后验估计：xk^=xk−^+Kk(zk−Hxk−^)误差协方差：pk=(I−KkH)pk−(7-2)
Note:当系统矩阵仅为A阵且为单位阵时，Kalman Filter则退化成低通滤波器
参考资料
标签：xk,Kalman,ek,Kk,Filter,tag,pk,hat 来源： https://blog.csdn.net/u013581448/article/details/118000571