关键词：递归、最优化、观测器
举例引入
对一个端口的电压，我们需要获取他的电压值，因此我们对其进行了多次测量，得到该端口电压的多个测量值（观测值） y 1 , y 2 , . . . , y k y_1,y_2,...,y_k y1,y2,...,yk 。
由于我们的测量设备存在的误差，这些测量值并不完全相同，那如何确定端口电压的真实值呢？我们可以很容易的想到，取所有测量值的平均值为我们认为的真实值（估计值）。
则有估计值
x
k
^
=
1
k
(
y
1
+
y
2
+
.
.
.
+
y
k
)
\hat{x_k} = \frac{1}{k} (y_1 + y_2 + ...+ y_k)
xk^=k1(y1+y2+...+yk)
对该式进行变式操作：
x
k
^
=
1
k
(
y
1
+
y
2
+
.
.
.
+
y
k
−
1
)
+
1
k
y
k
=
k
−
1
k
1
k
−
1
(
y
1
+
y
2
+
.
.
.
+
y
k
−
1
)
+
1
k
y
k
=
k
−
1
k
x
k
−
1
^
+
1
k
y
k
=
x
k
−
1
^
+
1
k
(
y
k
−
x
k
−
1
^
)
\begin{split} \hat{x_k} & = \frac{1}{k}(y_1 + y_2 + ...+y_{k-1})+\frac{1}{k}y_k \\ & = \frac{k-1}{k}\frac{1}{k-1}(y_1+y_2+...+y_{k-1}) + \frac{1}{k}y_k \\ & = \frac{k-1}{k}\hat{x_{k-1}} + \frac{1}{k}y_k \\ & = \hat{x_{k-1}} + \frac{1}{k}(y_k - \hat{x_{k-1}}) \end{split}
xk^=k1(y1+y2+...+yk−1)+k1yk=kk−1k−11(y1+y2+...+yk−1)+k1yk=kk−1xk−1^+k1yk=xk−1^+k1(yk−xk−1^)
可以看到，估计值
x
k
^
\hat{x_k}
xk^的表达式为一个递归式
当我们把上式第二项的系数
1
k
\frac{1}{k}
k1改为卡尔曼增益
K
k
K_k
Kk，就得到了一个卡尔曼滤波的基本公式：
x
k
^
=
x
k
−
1
^
+
K
k
(
y
k
−
x
k
−
1
^
)
\hat{x_k} =\hat{x_{k-1}} + K_k(y_k - \hat{x_{k-1}})
xk^=xk−1^+Kk(yk−xk−1^)
状态空间与观测器
观测器，这是一个现代控制理论的概念。所谓观测器，就是根据系统的可测量的输出信号，去估计系统不可直接测量的状态信号。
也可以认为观测器就是一种算法，一种通过测量值，去估计不可测量的状态值的估计算法，卡尔曼滤波就是这样一种算法。
在构建观测器之前，需要先对当前系统进行建模，构建出其状态空间方程。
状态空间方程分为状态方程与观测方程，状态方程一般为状态量的递归式，他表示系统状态随时间的迭代规律，观测方程一般为观测量与状态量的映射关系
一般的线性系统状态空间方程的矩阵形式可以表示为：
X
k
=
A
X
k
−
1
+
B
U
k
−
1
+
w
k
−
1
Y
k
=
C
X
k
+
D
U
k
+
v
k
\begin{split} X_k &= AX_{k-1}+BU_{k-1}+w_{k-1}\\ Y_k &= CX_k + D U_k + v_k\\ \end{split}
XkYk=AXk−1+BUk−1+wk−1=CXk+DUk+vk其中
X
k
X_k
Xk为状态量；
Y
k
Y_k
Yk为观测量；
U
k
U_k
Uk为输入量；
w
k
w_k
wk为过程噪声，表示系统未建模部分和系统扰动；
v
k
v_k
vk为观测噪声，表示测量传感器存在的误差。
卡尔曼滤波原理
对于上述状态空间方程
X
k
=
A
X
k
−
1
+
B
U
k
−
1
+
w
k
−
1
Y
k
=
C
X
k
+
D
U
k
+
v
k
(1)
\begin{split} X_k &= AX_{k-1}+BU_{k-1}+w_{k-1}\tag{1}\\ Y_k &= CX_k + D U_k + v_k\\ \end{split}
XkYk=AXk−1+BUk−1+wk−1=CXk+DUk+vk(1) 给出假设条件：
w
k
w_k
wk与
v
k
v_k
vk相互独立且均服从高斯分布，有
w
k
∼
(
0
,
Q
k
)
w_k \sim (0, Q_k)
wk∼(0,Qk) ，
v
k
∼
(
0
,
R
k
)
v_k\sim(0, R_k)
vk∼(0,Rk)
根据状态空间方程我们可以直接给出两个估计结果：
先验估计：
X
k
−
^
=
A
X
k
−
1
^
+
B
U
k
−
1
(2)
\hat{X_k^-} = A\hat{X_{k-1}}+BU_{k-1}\tag{2}
Xk−^=AXk−1^+BUk−1(2)
观测估计：
X
k
,
o
b
s
^
=
C
−
1
(
Y
k
−
D
U
k
)
(3)
\hat{X_{k,obs}} = C^{-1}(Y_k - DU_k)\tag{3}
Xk,obs^=C−1(Yk−DUk)(3)
由于噪声的存在，上述两个估计都不准确，卡尔曼滤波器要做的事就是借助这两个不准确的估计，得到一个更为准确的估计。
根据举例引入给出的卡尔曼滤波基本公式可得：
后验估计：
X
k
^
=
X
k
−
^
+
G
k
(
X
k
,
o
b
s
−
X
k
−
^
^
)
\hat{X_k} = \hat{X_k^-} + G_k(\hat{X_{k,obs} - \hat{X_k^-}})
Xk^=Xk−^+Gk(Xk,obs−Xk−^^)
我们一般令
G
k
=
K
k
C
G_k = K_kC
Gk=KkC，再把
X
k
,
o
b
s
^
\hat{X_{k,obs}}
Xk,obs^代入得到：
X
k
^
=
X
k
−
^
+
K
k
(
Y
k
−
D
U
k
−
C
X
k
−
^
)
(4)
\hat{X_k} = \hat{X_k^-} + K_k(Y_k - DU_k - C\hat{X_k^-})\tag{4}
Xk^=Xk−^+Kk(Yk−DUk−CXk−^)(4)
此时的算法目标变成：找到一个 K k K_k Kk，使得 X k ^ \hat{X_k} Xk^最接近状态量的真实值 X k X_k Xk。 = > m i n { e k = X k − X k ^ } e k ∼ ( 0 , P k ) = > m i n { σ 2 ( e k ) } = > m i n { t r ( P k ) } \begin{split} => &min\{e_k = X_k - \hat{X_k}\} \qquad e_k\sim(0, P_k) \\ => &min\{\sigma^2(e_k)\} \\ => &min\{tr(P_k)\} \end{split} =>=>=>min{ek=Xk−Xk^}ek∼(0,Pk)min{σ2(ek)}min{tr(Pk)}
卡尔曼增益
K
k
K_k
Kk表达式的数学推导
跳过
所定义的
e
k
=
X
k
−
X
k
^
e_k = X_k - \hat{X_k}
ek=Xk−Xk^
代入Eq.(4),(1)得：
e
k
=
X
k
−
X
k
−
^
−
K
k
(
C
X
k
+
v
k
−
C
X
k
−
)
e_k = X_k - \hat{X_k^-} - K_k(CX_k + v_k - CX_k^-)
ek=Xk−Xk−^−Kk(CXk+vk−CXk−)
定义：
e
k
−
=
X
k
−
X
k
−
e_k^- = X_k - X_k^-
ek−=Xk−Xk−
则有：
e
k
=
e
k
−
−
K
k
(
C
e
k
−
+
v
k
)
=
(
I
−
K
k
C
)
e
k
−
−
K
k
v
k
(5)
\begin{split} e_k &= e_k^- -K_k(Ce_k^- + v_k)\tag{5}\\ &=(I-K_kC)e_k^- - K_kv_k \end{split}
ek=ek−−Kk(Cek−+vk)=(I−KkC)ek−−Kkvk(5)
协方差矩阵
P
k
P_k
Pk有计算公式
P
k
=
E
[
e
k
e
k
T
]
P_k = E[e_ke_k^T]
Pk=E[ekekT]
代入Eq.(5)得：
P
k
=
E
[
e
k
e
k
T
]
=
E
[
{
(
I
−
K
k
C
)
e
k
−
−
K
k
v
k
}
{
(
I
−
K
k
C
)
e
k
−
−
K
k
v
k
}
T
]
=
E
[
{
(
I
−
K
k
C
)
e
k
−
−
K
k
v
k
}
{
e
k
−
T
(
I
−
K
k
C
)
T
−
v
k
T
K
k
T
}
]
=
E
[
(
I
−
K
k
C
)
e
k
−
e
k
−
T
(
I
−
K
k
C
)
T
]
−
E
[
(
I
−
K
k
C
)
e
k
−
v
k
T
K
k
T
]
−
E
[
K
k
v
k
e
k
−
T
(
I
−
K
k
C
)
T
]
+
E
[
K
k
v
k
v
k
T
K
k
T
]
=
(
I
−
K
k
C
)
E
[
e
k
−
e
k
−
T
]
(
I
−
K
k
C
)
T
−
(
I
−
K
k
C
)
E
[
e
k
−
]
E
[
v
k
T
]
K
k
T
−
K
k
E
[
v
k
]
E
[
e
k
−
T
]
(
I
−
K
k
C
)
T
+
K
k
E
[
v
k
v
k
T
]
K
k
T
\begin{split} P_k = &E[e_ke_k^T]\\ = &E[\{(I-K_kC)e_k^--K_kv_k\}\{(I-K_kC)e_k^--K_kv_k\}^T]\\ = &E[\{(I-K_kC)e_k^--K_kv_k\}\{e_k^{-T}(I-K_kC)^T - v_k^TK_k^T\}]\\ = &E[(I-K_kC)e_k^-e_k^{-T}(I-K_kC)^T] -E[(I-K_kC)e_k^-v_k^TK_k^T] -E[K_kv_ke_k^{-T}(I-K_kC)^T] +E[K_kv_kv_k^TK_k^T]\\ = &(I-K_kC)E[e_k^-e_k^{-T}](I-K_kC)^T -(I-K_kC)E[e_k^-]E[v_k^T]K_k^T -K_k E[v_k]E[e_k^{-T}](I-K_kC)^T +K_kE[v_kv_k^T]K_k^T \end{split}
Pk=====E[ekekT]E[{(I−KkC)ek−−Kkvk}{(I−KkC)ek−−Kkvk}T]E[{(I−KkC)ek−−Kkvk}{ek−T(I−KkC)T−vkTKkT}]E[(I−KkC)ek−ek−T(I−KkC)T]−E[(I−KkC)ek−vkTKkT]−E[Kkvkek−T(I−KkC)T]+E[KkvkvkTKkT](I−KkC)E[ek−ek−T](I−KkC)T−(I−KkC)E[ek−]E[vkT]KkT−KkE[vk]E[ek−T](I−KkC)T+KkE[vkvkT]KkT
v
k
,
e
k
−
v_k,e_k^-
vk,ek−的均值都为0，即
E
[
v
k
]
=
0
,
E
[
e
k
−
]
=
0
E[v_k]=0,E[e_k^-]=0
E[vk]=0,E[ek−]=0;
E
[
e
k
−
e
k
−
T
]
E[e_k^-e_k^{-T}]
E[ek−ek−T]即为
e
k
−
e_k^-
ek−的协方差矩阵，
P
k
−
P_k^-
Pk−
E
[
v
k
v
k
T
]
E[v_kv_k^T]
E[vkvkT]即为
v
k
v_k
vk的协方差矩阵，
R
k
R_k
Rk
则有：
P
k
=
(
I
−
K
k
C
)
P
k
−
(
I
−
K
k
C
)
T
−
0
−
0
+
K
k
R
k
K
k
T
=
(
P
k
−
−
K
k
C
P
k
−
)
(
I
−
C
T
K
k
T
)
+
K
k
R
k
K
k
T
=
P
k
−
−
P
k
−
C
T
K
k
T
−
K
k
C
P
k
−
+
K
k
C
P
k
−
C
T
K
k
T
+
K
k
R
k
K
k
T
(6)
\begin{split} P_k &= (I-K_kC)P_k^-(I-K_kC)^T - 0 - 0 + K_kR_kK_k^T\\ &= (P_k^- - K_kCP_k^-)(I-C^TK_k^T) + K_kR_kK_k^T\\ &= P_k^- - P_k^-C^TK_k^T - K_kCP_k^- + K_kCP_k^-C^TK_k^T + K_kR_kK_k^T\tag{6} \end{split}
Pk=(I−KkC)Pk−(I−KkC)T−0−0+KkRkKkT=(Pk−−KkCPk−)(I−CTKkT)+KkRkKkT=Pk−−Pk−CTKkT−KkCPk−+KkCPk−CTKkT+KkRkKkT(6)
得到
P
k
P_k
Pk的表达式，计算它的迹
t
r
(
P
k
)
tr(P_k)
tr(Pk)
t
r
(
P
k
)
=
t
r
(
P
k
−
)
−
t
r
(
P
k
−
C
T
K
k
T
)
−
t
r
(
K
k
C
P
k
−
)
+
t
r
(
K
k
C
P
k
−
C
T
K
k
T
)
+
t
r
(
K
k
R
k
K
k
T
)
tr(P_k) = tr(P_k^-) -tr(P_k^-C^TK_k^T) -tr(K_kCP_k^-) +tr(K_kCP_k^-C^TK_k^T) +tr(K_kR_kK_k^T)
tr(Pk)=tr(Pk−)−tr(Pk−CTKkT)−tr(KkCPk−)+tr(KkCPk−CTKkT)+tr(KkRkKkT)
要求得 t r ( P k ) tr(P_k) tr(Pk)的最小值，计算其极小值点，即对 K k K_k Kk导数为0的点
引理：矩阵计算公式有： d t r ( A B ) d A = B T d t r ( A B A T ) d A = 2 A B \frac{d\,tr(AB)}{d\,A} =B^T \qquad \frac{d\,tr(ABA^T)}{d\,A}=2AB dAdtr(AB)=BTdAdtr(ABAT)=2AB
观察
t
r
(
P
k
)
tr(P_k)
tr(Pk)表达式可知：式子第二项与第三项互为转置，因此他们的迹相等；第后四项均符合引理给出公式对应的形式
所以有：
d
t
r
(
P
k
)
d
K
k
=
0
−
2
P
k
−
T
C
T
+
2
K
k
C
P
k
−
C
T
+
2
K
k
R
k
=
0
2
K
k
C
P
k
−
C
T
+
2
K
k
R
k
=
2
P
k
−
T
C
T
\begin{split} \frac{d\,tr(P_k)}{d\,K_k} = 0 - 2P_k^{-T}C^T + 2K_kCP_k^-C^T + 2K_kR_k = 0\\ 2K_kCP_k^-C^T + 2K_kR_k=2P_k^{-T}C^T\\ \end{split}
dKkdtr(Pk)=0−2Pk−TCT+2KkCPk−CT+2KkRk=02KkCPk−CT+2KkRk=2Pk−TCT
因为协方差矩阵均为对称矩阵，所以
P
k
−
T
=
P
k
−
P_k^{-T} = P_k^-
Pk−T=Pk−
得到
t
r
(
P
k
)
tr(P_k)
tr(Pk)为最小值时，卡尔曼增益
K
k
K_k
Kk的表达式：
K
k
=
P
k
−
C
T
(
C
P
k
−
C
T
+
R
k
)
−
1
(7)
K_k=P_k^-C^T(CP_k^-C^T+R_k)^{-1} \tag{7}
Kk=Pk−CT(CPk−CT+Rk)−1(7)
可以发现，卡尔曼增益
K
k
K_k
Kk的表达式中，先验估计的协方差矩阵
P
k
−
P_k^-
Pk−还没有得到，因此需要计算先验估计的协方差矩阵
P
k
−
P_k^-
Pk−的表达式。
先验协方差矩阵
P
k
−
P_k^-
Pk−表达式的数学推导
跳过
所定义的
e
k
−
=
X
k
−
X
k
−
^
e_k^- = X_k - \hat{X_k^-}
ek−=Xk−Xk−^
代入Eq.(1),(2)得：
e
k
−
=
A
X
k
−
1
+
B
U
k
−
1
+
w
k
−
1
−
A
X
k
−
1
^
−
B
U
k
−
1
=
A
e
k
−
1
+
w
k
−
1
(8)
\begin{split} e_k^- &= AX_{k-1} + BU_{k-1} + w_{k-1} - A\hat{X_{k-1}} - BU_{k-1}\\ &=Ae_{k-1} + w_{k-1} \tag{8} \end{split}
ek−=AXk−1+BUk−1+wk−1−AXk−1^−BUk−1=Aek−1+wk−1(8)
协方差矩阵
P
k
−
P_k^-
Pk−有计算公式
P
k
−
=
E
[
e
k
−
e
k
−
T
]
P_k^- = E[e_k^-e_k^{-T}]
Pk−=E[ek−ek−T]
代入Eq.(8)得：
P
k
−
=
E
[
e
k
−
e
k
−
T
]
=
E
[
(
A
e
k
−
1
+
w
k
−
1
)
(
A
e
k
−
1
+
w
k
−
1
)
T
]
=
E
[
(
A
e
k
−
1
+
w
k
−
1
)
(
e
k
−
1
T
A
T
+
w
k
−
1
T
)
]
=
A
E
[
e
k
−
1
e
k
−
1
T
]
A
T
+
A
E
[
e
k
−
1
]
E
[
w
k
−
1
T
]
+
E
[
w
k
−
1
]
E
[
e
k
−
1
T
]
A
T
+
E
[
w
k
−
1
w
k
−
1
T
]
\begin{split} P_k^- &= E[e_k^-e_k^{-T}]\\ &= E[(Ae_{k-1} + w_{k-1})(Ae_{k-1} + w_{k-1})^T]\\ &= E[(Ae_{k-1} + w_{k-1})(e_{k-1}^TA^T+w_{k-1}^T)]\\ &= AE[e_{k-1}e_{k-1}^T]A^T + AE[e_{k-1}]E[w_{k-1}^T]+E[w_{k-1}]E[e_{k-1}^T]A^T+E[w_{k-1}w_{k-1}^T] \end{split}
Pk−=E[ek−ek−T]=E[(Aek−1+wk−1)(Aek−1+wk−1)T]=E[(Aek−1+wk−1)(ek−1TAT+wk−1T)]=AE[ek−1ek−1T]AT+AE[ek−1]E[wk−1T]+E[wk−1]E[ek−1T]AT+E[wk−1wk−1T]
w
k
,
e
k
w_k,e_k
wk,ek的均值都为0，即
E
[
w
k
]
=
0
,
E
[
e
k
]
=
0
E[w_k]=0,E[e_k]=0
E[wk]=0,E[ek]=0;
E
[
e
k
e
k
T
]
E[e_ke_k^T]
E[ekekT]即为
e
k
e_k
ek的协方差矩阵，
P
k
P_k
Pk
E
[
w
k
w
k
T
]
E[w_kw_k^T]
E[wkwkT]即为
w
k
w_k
wk的协方差矩阵，
Q
k
Q_k
Qk
则有：
P
k
−
=
A
P
k
−
1
A
T
+
0
+
0
+
Q
k
−
1
P_k^- = AP_{k-1}A^T + 0 + 0 + Q_{k-1}
Pk−=APk−1AT+0+0+Qk−1
即：
P
k
−
=
A
P
k
−
1
A
T
+
Q
k
−
1
(9)
P_k^- = AP_{k-1}A^T + Q_{k-1}\tag{9}
Pk−=APk−1AT+Qk−1(9)
由Eq.(9)可知，计算
P
k
−
P_k^-
Pk−需要上一时刻的后验协方差矩阵
P
k
−
1
P_{k-1}
Pk−1，因此还需要后验协方差矩阵
P
k
P_k
Pk的表达式以时刻更新
P
k
P_k
Pk。
Eq.(6)给出了
P
k
P_k
Pk的表达式：
P
k
=
P
k
−
−
P
k
−
C
T
K
k
T
−
K
k
C
P
k
−
+
K
k
C
P
k
−
C
T
K
k
T
+
K
k
R
k
K
k
T
P_k= P_k^- - P_k^-C^TK_k^T - K_kCP_k^- + K_kCP_k^-C^TK_k^T + K_kR_kK_k^T
Pk=Pk−−Pk−CTKkT−KkCPk−+KkCPk−CTKkT+KkRkKkT
将上式第四项和第五项合并得：
K
k
(
C
P
k
−
C
T
+
R
k
)
K
k
T
K_k(CP_k^-C^T+R_k)K_k^T
Kk(CPk−CT+Rk)KkT
代入Eq.(7)
（
K
k
（K_k
（Kk的表达式）得：
P
k
−
C
T
K
k
T
P_k^-C^TK_k^T
Pk−CTKkT，可与第二项相消。
因此得到后验协方差矩阵
P
k
P_k
Pk的更新式为：
P
k
=
(
I
−
K
k
C
)
P
k
−
(10)
P_k = (I - K_kC)P_k^-\tag{10}
Pk=(I−KkC)Pk−(10)
卡尔曼滤波流程
整理Eq.(1),(2),(4),(7),(9),(10)可得卡尔曼滤波算法的完整流程
状态空间方程：
X k = A X k − 1 + B U k − 1 + w k − 1 Y k = C X k + D U k + v k \begin{split} X_k &= AX_{k-1}+BU_{k-1}+w_{k-1}\\ Y_k &= CX_k + D U_k + v_k\\ \end{split} XkYk=AXk−1+BUk−1+wk−1=CXk+DUk+vk
假设条件：
w k \qquad w_k wk与 v k v_k vk相互独立且均服从高斯分布，有 w k ∼ ( 0 , Q k ) w_k \sim (0, Q_k) wk∼(0,Qk) ， v k ∼ ( 0 , R k ) v_k\sim(0, R_k) vk∼(0,Rk)
初始条件：
X 0 ^ = E [ X 0 ] P 0 = E [ ( X 0 − X 0 ^ ) ( X 0 − X 0 ^ ) T ] \begin{split} \hat{X_0} &= E[X_0]\\ P_0 &= E[(X_0-\hat{X_0})(X_0-\hat{X_0})^T] \end{split} X0^P0=E[X0]=E[(X0−X0^)(X0−X0^)T]
循环：
预测： X k − ^ = A X k − 1 ^ + B U k − 1 P k − = A P k − 1 A T + Q k − 1 卡尔曼增益： K k = P k − C T ( C P k − C T + R k ) − 1 校正： X k ^ = X k − ^ + K k ( Y k − C X k − ^ − D U k ) P k = ( I − K k C ) P k − \begin{split} 预测： \hat{X_k^-}& = A\hat{X_{k-1}}+BU_{k-1}\\ P_k^- &= AP_{k-1}A^T + Q_{k-1}\\ \\ 卡尔曼增益：K_k &= P_k^-C^T(CP_k^-C^T+R_k)^{-1}\\ \\ 校正：\hat{X_k} &= \hat{X_k^-} + K_k(Y_k - C\hat{X_k^-} - DU_k)\\ P_k &= (I - K_kC)P_k^- \end{split} 预测：Xk−^Pk−卡尔曼增益：Kk校正：Xk^Pk=AXk−1^+BUk−1=APk−1AT+Qk−1=Pk−CT(CPk−CT+Rk)−1=Xk−^+Kk(Yk−CXk−^−DUk)=(I−KkC)Pk−
Reference：
【卡尔曼滤波器】-- DR_can
详解卡尔曼滤波原理
704
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
