由
k
−
1
k-1
k−1到
k
k
k时刻,系统状态方程
x
k
=
A
k
x
k
−
1
+
B
k
u
k
+
C
w
k
(1)
x_k=A_kx_{k-1}+B_ku_k+Cw_k\tag{1}
xk=Akxk−1+Bkuk+Cwk(1)
系统观测方程
y
k
=
H
k
x
k
+
v
k
(2)
y_k=H_kx_k+v_k\tag{2}
yk=Hkxk+vk(2)
其中,
x
k
x_k
xk为真实值;
A
k
A_k
Ak为状态转移矩阵;
u
k
u_k
uk为系统输入;
B
k
B_k
Bk为输入增益矩阵;
y
k
y_k
yk为测量值;
H
k
H_k
Hk为观测矩阵;
w
k
∼
N
(
0
,
Q
)
w_k\sim N\left(0,Q \right)
wk∼N(0,Q)为过程高斯噪声,
Q
Q
Q为其协方差阵;
C
C
C为过程噪声增益矩阵;
v
k
∼
N
(
0
,
R
)
v_k\sim N\left(0,R \right)
vk∼N(0,R)为测量高斯噪声,
R
R
R为其协方差阵;
w
k
w_{k}
wk和
v
k
v_{k}
vk不相关。
注意:式(1)是实际系统方程,式(2)是得用实际传感器来测量。因为这两个式子都有噪声,所以不可能准确知道每一时刻的 x k x_k xk。只能知道个大概范围,下面要做的就是得到一个最好 x ^ k \hat{x}_k x^k来估计 x k x_k xk。
先说状态方程这部分。
假设
x
0
∼
N
(
x
^
0
,
P
0
)
x_0\sim N\left(\hat{x}_0,P_0 \right)
x0∼N(x^0,P0),由于我们前一时刻对当前时刻的状态预测时不知道
w
k
w_k
wk是多少(但是知道
u
k
u_k
uk,因为这是我们输入的),所以我们就先产生一个状态先验预测,即
x
^
k
′
=
A
k
x
^
k
−
1
+
B
k
u
k
(3)
\hat{x}_k'=A_k\hat{x}_{k-1}+B_ku_k\tag{3}
x^k′=Akx^k−1+Bkuk(3)
同样地,这个预测是有不确定性的,也就是会有一个协方差矩阵来描述不确定性。因为是对先验状态的描述,所以叫先验协方差矩阵,即
P
k
′
=
A
k
P
k
−
1
A
k
T
+
C
Q
C
T
(4)
P_k'=A_kP_{k-1}A_k^T+CQC^T\tag{4}
Pk′=AkPk−1AkT+CQCT(4)
需要解释一下这个式(4)的由来,先说式(4)的第一项,当前时刻的先验状态是从前一时刻的后验来的,式(3)可知;先说式(4)的第二项,我们是知道
w
k
w_k
wk的统计特性的。所以二者相加就是后一时刻的先验协方差矩阵。
注意:这里解释一下为什么是上面这样。假设有一个随机向量 x ∼ N ( x ^ , P ) x\sim N\left(\hat{x},P \right) x∼N(x^,P),那么随机向量的变换 A x + B u ∼ N ( A x ^ + B u , A P A T ) Ax+Bu\sim N(A\hat{x}+Bu,APA^T) Ax+Bu∼N(Ax^+Bu,APAT)。同样,假设有另一个随机向量 w ∼ N ( 0 , Q ) w\sim N(0,Q) w∼N(0,Q), C w ∼ N ( 0 , C Q C T ) Cw\sim N(0,CQC^T) Cw∼N(0,CQCT)。两者的和 A x + B u + C w ∼ N ( A x ^ + B u , A P A T + C Q C T ) Ax+Bu+Cw\sim N(A\hat{x}+Bu,APA^T+CQC^T) Ax+Bu+Cw∼N(Ax^+Bu,APAT+CQCT)。能这样算的前提条件是这两个随机向量都是高斯不相关的,刚好这个条件满足。具体详细的推导可以参考数理统计、随机过程、矩阵论方面的书。
总结:前一时刻的真实后验 x k − 1 ∼ N ( x ^ k − 1 , P k − 1 ) x_{k-1}\sim N\left(\hat{x}_{k-1},P_{k-1} \right) xk−1∼N(x^k−1,Pk−1),当前时刻的真实先验 x k ′ ∼ N ( x ^ k ′ , P k ′ ) x'_k\sim N(\hat{x}_k',P_k') xk′∼N(x^k′,Pk′)
再说观测方程这部分。
由于我们对当前时刻的观测时不知道
v
k
v_k
vk是多少,而且不知道真实的
x
k
x_k
xk,所以我们就对式(3)观测,先产生一个观测预测,这个预测也是有不确定性的,即
y
k
′
∼
N
(
H
k
x
^
k
′
,
H
k
P
k
′
H
k
T
)
(5)
y_k'\sim N(H_k\hat{x}_k',H_kP_k'H_k^T)\tag{5}
yk′∼N(Hkx^k′,HkPk′HkT)(5)
由观测方程(2),假设传感器读数分布的均值等于我们观察到传感器的读数
y
k
y_k
yk,其协方差为
R
R
R。
这样一来,我们有了两个高斯分布:一个围绕通过状态转移预测的平均值,另一个围绕实际传感器读数。我们的目标是融合这两个分布,也就是相交的部分,很自然就是二者的乘积。
融合。
首先考虑一维高斯情况:一个均值为
μ
\mu
μ,方差
σ
2
\sigma^2
σ2为的高斯分布的形式为:
N
(
x
;
μ
,
σ
)
=
1
2
π
σ
2
e
−
(
x
−
μ
)
2
2
σ
2
(6)
N(x;\mu,\sigma)=\frac{1}{\sqrt{2\pi\sigma^2}}e^{-\frac{(x-\mu)^2}{2\sigma^2}}\tag{6}
N(x;μ,σ)=2πσ21e−2σ2(x−μ)2(6)
我们想知道将两个高斯曲线相乘会发生什么,就是融合后的
μ
′
\mu'
μ′和
σ
′
\sigma'
σ′是多少(这里有个前提,因为知道两个高斯相乘还是高斯)。
N
(
x
;
μ
0
,
σ
0
)
N
(
x
;
μ
1
,
σ
1
)
=
N
(
x
;
μ
′
,
σ
′
)
(7)
N(x;\mu_0,\sigma_0)N(x;\mu_1,\sigma_1)=N(x;\mu',\sigma')\tag{7}
N(x;μ0,σ0)N(x;μ1,σ1)=N(x;μ′,σ′)(7)
将式(6)带入式(7),我们可以得到新的高斯分布的均值和方差如下所示:
μ
′
=
μ
0
+
σ
0
2
(
μ
1
−
μ
0
)
σ
0
2
+
σ
1
2
=
μ
0
+
k
(
μ
1
−
μ
0
)
σ
′
2
=
σ
0
2
−
σ
0
4
σ
0
2
+
σ
1
2
=
σ
0
2
−
k
σ
0
2
k
≜
σ
0
2
σ
0
2
+
σ
1
2
(8)
\begin{aligned} \mu'&=\mu_0+\frac{\sigma_0^2(\mu_1-\mu_0)}{\sigma_0^2+\sigma_1^2}=\mu_0+k(\mu_1-\mu_0)\\ \sigma'^2&=\sigma_0^2-\frac{\sigma_0^4}{\sigma_0^2+\sigma_1^2}=\sigma_0^2-k{\sigma_0^2}\\ k&\triangleq\frac{\sigma_0^2}{\sigma_0^2+\sigma_1^2} \end {aligned} \tag{8}
μ′σ′2k=μ0+σ02+σ12σ02(μ1−μ0)=μ0+k(μ1−μ0)=σ02−σ02+σ12σ04=σ02−kσ02≜σ02+σ12σ02(8)
详细推导见这里和这里。
这样一来,公式的形式就简单多了!我们顺势将上式的矩阵形式写在下面:
μ
′
=
μ
0
+
K
(
μ
1
−
μ
0
)
Σ
′
=
Σ
0
−
K
Σ
0
K
≜
Σ
0
(
Σ
0
+
Σ
1
)
−
1
(9)
\begin{aligned} \boldsymbol{\mu}'&=\boldsymbol{\mu}_0+\mathcal{K}(\boldsymbol{\mu}_1-\boldsymbol{\mu}_0)\\ \Sigma'&=\Sigma_0-\mathcal{K}{\Sigma_0}\\ \mathcal{K}&\triangleq \Sigma_0(\Sigma_0+\Sigma_1)^{-1} \end {aligned} \tag{9}
μ′Σ′K=μ0+K(μ1−μ0)=Σ0−KΣ0≜Σ0(Σ0+Σ1)−1(9)
其中
Σ
′
\Sigma'
Σ′表示新高斯分布的协方差矩阵,
μ
′
\boldsymbol{\mu}'
μ′是每个维度的均值。
现在开始融合:
我们有两个高斯分布,一个是我们的观测预测
(
μ
0
,
Σ
0
)
=
(
H
k
x
^
k
′
,
H
k
P
k
′
H
k
T
)
(\boldsymbol{\mu}_0,\Sigma_0)=(H_k\hat{x}_k',H_kP_k'H_k^T)
(μ0,Σ0)=(Hkx^k′,HkPk′HkT),另外一个是实际的观测(传感器读数)
(
μ
1
,
Σ
1
)
=
(
y
k
,
R
)
(\mu_1,\Sigma_1)=(y_k,R)
(μ1,Σ1)=(yk,R),我们将这两个高斯分布带入公式(9)中就可以得到二者相乘的重叠区域:
H
k
x
^
k
=
H
k
x
^
k
′
+
K
k
(
y
k
−
H
k
x
^
k
′
)
H
k
P
k
H
k
T
=
H
k
P
k
′
H
k
T
−
K
k
H
k
P
k
′
H
k
T
K
k
≜
H
k
P
k
′
H
k
T
(
H
k
P
k
′
H
k
T
+
R
)
−
1
(10)
\begin{aligned} H_k\hat{x}_k&=H_k\hat{x}_k'+\mathcal{K}_k(y_k-H_k\hat{x}_k')\\ H_kP_kH_k^T&=H_kP_k'H_k^T-\mathcal{K}_kH_kP_k'H_k^T\\ \mathcal{K}_k&\triangleq H_kP_k'H_k^T(H_kP_k'H_k^T+R)^{-1} \end{aligned} \tag{10}
Hkx^kHkPkHkTKk=Hkx^k′+Kk(yk−Hkx^k′)=HkPk′HkT−KkHkPk′HkT≜HkPk′HkT(HkPk′HkT+R)−1(10)
由式(10)可得:
x
^
k
=
x
^
k
′
+
K
k
(
y
k
−
H
k
x
^
k
′
)
P
k
=
P
k
′
−
K
k
H
k
P
k
′
K
k
≜
P
k
′
H
k
T
(
H
k
P
k
′
H
k
T
+
R
)
−
1
(11)
\begin{aligned} \hat{x}_k&=\hat{x}_k'+K_k(y_k-H_k\hat{x}_k')\\ P_k&=P_k'-K_kH_kP_k'\\ K_k&\triangleq P_k'H_k^T(H_kP_k'H_k^T+R)^{-1} \end{aligned} \tag{11}
x^kPkKk=x^k′+Kk(yk−Hkx^k′)=Pk′−KkHkPk′≜Pk′HkT(HkPk′HkT+R)−1(11)
式(3)(4)(11)就是完整的KF。
最后说一下为什么协方差矩阵可以表示不确定性。以三维空间,也就是三维协方差矩阵为例:随机向量的均值表示空间一个确定点;将协方差矩阵特征值分解可以得到三个正交的单位向量,对应的三个特征值对该方向的单位向量进行放大或缩小,这样可以生成一个三维椭球。随机向量的所有可能在空间的点有99.74%在以均值点为中心的椭球内。