引言
虽然学了一段时间的卡尔曼滤波，也阅读过相关的论文、书籍，并作过卡尔曼滤波器核心公式的推导。但是，对于如何从贝叶斯估计逐步推导，得到标准卡尔曼滤波公式？这其中的联系，笔者却一直说不清，道不明。近日，在阅读Anton.J.Haug编写的"Bayesian Estimation and Tracking"一书时，总算对这一个过程有了些许了解。现在选取了书中的部分内容，解释从贝叶斯估计到卡尔曼滤波器的推导过程，希望可以帮到同样有此疑惑的朋友。若有书写不对的地方，欢迎提出指正。
一.贝叶斯估计基本概念
1.贝叶斯估计
贝叶斯估计是基于观测向量 z z z，对一段连续且未知的参数向量 x x x的客观估计。我们假设随机向量 x x x具有已知的先验概率密度函数 p ( x ) p(x) p(x)。先验概率密度函数 p ( x ) p(x) p(x)包含在获取任何观测信息前，关于参数 x x x的所有已知或未知的信息。假设 x x x的真值是已知的，那么我们可以通过似然函数 p ( z ∣ x ) p(z|x) p(z∣x)得到 z z z的概率密度，并且求得 z z z的统计特性。
假设我们在某次针对
x
x
x的实验中，已知随机向量
z
z
z。根据贝叶斯准则，可以得到
x
x
x的后验概率密度函数
p
(
x
∣
z
)
=
p
(
z
∣
x
)
p
(
x
)
p
(
z
)
=
p
(
z
,
x
)
∫
p
(
z
∣
x
)
p
(
x
)
d
x
(
1
)
p(x|z)=\frac{p(z|x)p(x)}{p(z)}=\frac{p(z,x)}{\int{p(z|x)p(x)dx}}(1)
p(x∣z)=p(z)p(z∣x)p(x)=∫p(z∣x)p(x)dxp(z,x)(1)
其中，
p
(
z
,
x
)
=
p
(
z
∣
x
)
p
(
x
)
(
2
)
p(z,x)=p(z|x)p(x)(2)
p(z,x)=p(z∣x)p(x)(2)
不难发现，在贝叶斯框架下，后验概率密度考虑了所有观测信息情况。
以上是对贝叶斯公式做了简单的回顾。现在让我们回归初心，我们希望可以利用贝叶斯公式解决估计问题。在估计问题中，贝叶斯公式中的 x x x代表着我们要估计的变量， z z z则代表着观测值， x ^ \hat x x^代表着我们对变量 x x x的估计值。单看上式，我们似乎离希望得到的估计值 x ^ \hat x x^的闭式解还差了十万八千里。难道利用后验概率密度就能求得我们希望的得到的估计值 x ^ \hat x x^了吗？
不错，只要待估计参数 x x x是存在的，我们都可以利用后验概率密度去求得该参数的估计值。(关于这一点，在下一节中有推导)
2.点估计
假设点估计 x ^ \hat x x^是我们在根据已有观测值情况下得到的估计值。现在，我们要在贝叶斯框架下，利用估计器来对 x ^ \hat x x^进行估计。
一般而言，所使用的估计器不同，最后得到的估计值
x
^
\hat x
x^也不尽相同。我们使用代价函数来区分不同的估计器。常用的代价函数有最小均方误差、最大信噪比、线性约束最小方差、极大极小等。在这里，我们定义代价函数为
L
(
x
^
,
x
)
L(\hat x,x)
L(x^,x)。当差值
x
^
−
x
\hat x-x
x^−x越大，代价函数
L
(
x
^
,
x
)
L(\hat x,x)
L(x^,x)的值就越大，相反，代价函数
L
(
x
^
,
x
)
L(\hat x,x)
L(x^,x)的值越小，说明我们估计的越精确。理论上，代价函数的值都是正数，其最小值为0。并且定义贝叶斯风险
R
R
R是代价函数的期望，记为
R
:
=
E
{
L
(
x
^
,
x
)
}
(
3
)
R:=E\left \{ L(\hat x,x)\right \} (3)
R:=E{L(x^,x)}(3)
因为我们是根据观测值
z
z
z来计算估计值
x
^
\hat x
x^的，因此将
x
^
\hat x
x^可以写为
x
^
(
z
)
\hat x(z)
x^(z)，即
x
^
\hat x
x^是基于
z
z
z的函数。正如上文所说，代价函数
L
(
x
^
,
x
)
L(\hat x,x)
L(x^,x)的值越小，说明我们估计的越精确。那么，当我们最小化贝叶斯风险
R
R
R时，就获得了参数
x
x
x的最优估计值，利用概率论中，求解均值的公式可以得到
x
^
(
z
)
=
a
r
g
m
i
n
x
∗
(
z
)
∫
L
(
x
∗
(
z
)
,
x
)
p
(
x
,
z
)
d
x
d
z
(
4
)
\hat x(z)=argmin_{x^{*}(z)}\int L(x^{*}(z),x)p(x,z)dxdz (4)
x^(z)=argminx∗(z)∫L(x∗(z),x)p(x,z)dxdz(4)
类似于式(2)，我们可以得到
p
(
x
∣
z
)
p
(
z
)
=
p
(
x
,
z
)
p(x|z)p(z)=p(x,z)
p(x∣z)p(z)=p(x,z),并带入上式，进一步得到
x
^
(
z
)
=
a
r
g
m
i
n
x
∗
(
z
)
∫
[
∫
L
(
x
∗
(
z
)
,
x
)
p
(
x
∣
z
)
d
x
]
p
(
z
)
d
z
=
a
r
g
m
i
n
x
∗
(
z
)
∫
L
(
x
∗
(
z
)
,
x
)
p
(
x
∣
z
)
d
x
(
5
)
\hat x(z)=argmin_{x^{*}(z)}\int \left [ \int L(x^{*}(z),x)p(x|z)dx\right ]p(z)dz\\=argmin_{x^{*}(z)}\int L(x^{*}(z),x)p(x|z)dx(5)
x^(z)=argminx∗(z)∫[∫L(x∗(z),x)p(x∣z)dx]p(z)dz=argminx∗(z)∫L(x∗(z),x)p(x∣z)dx(5)
因此，贝叶斯估计问题就转化为了，使用不同的代价函数，来求解式(5)的解。
这里，我们使用最小均方误差准则，首先定义误差
ϵ
x
:
=
x
−
x
^
\epsilon ^{x}:=x-\hat x
ϵx:=x−x^,则最小均方误差准则下，代价函数可以写为
L
(
ϵ
x
)
=
(
ϵ
x
)
T
M
ϵ
x
(
6
)
L(\epsilon ^{x})=(\epsilon ^{x})^{T}M\epsilon ^{x}(6)
L(ϵx)=(ϵx)TMϵx(6)
其中，
M
M
M是已知的，且独立于
x
x
x的平方加权矩阵。将其带入式(5)可得，
x
^
(
z
)
=
a
r
g
m
i
n
x
∗
(
z
)
∫
(
x
−
x
∗
)
T
M
(
x
−
x
∗
)
p
(
x
∣
z
)
d
x
(
7
)
\hat x(z)=argmin_{x^{*}(z)}\int (x-x^{*})^{T}M(x-x^{*})p(x|z)dx(7)
x^(z)=argminx∗(z)∫(x−x∗)TM(x−x∗)p(x∣z)dx(7)
为了得到上式的最小值，我们求解式(6)关于参数
x
∗
x^{*}
x∗的偏导，
d
d
x
∗
∫
(
x
−
x
∗
)
T
M
(
x
−
x
∗
)
p
(
x
∣
z
)
d
x
=
−
2
M
∫
x
p
(
x
∣
z
)
d
x
+
2
M
x
∗
∫
p
(
x
∣
z
)
d
x
(
8
)
\begin{matrix} \frac{d}{dx^{*}}\int (x-x^{*})^{T}M(x-x^{*})p(x|z)dx\\ =-2M\int xp(x|z)dx+2Mx^{*}\int p(x|z)dx \end{matrix}(8)
dx∗d∫(x−x∗)TM(x−x∗)p(x∣z)dx=−2M∫xp(x∣z)dx+2Mx∗∫p(x∣z)dx(8)
令式(8)等于0，并且注意，式(8)中第二个积分等于1，则可以得到下式
x
^
m
s
=
∫
x
p
(
x
∣
z
)
d
x
:
=
E
{
x
∣
z
}
(
9
)
\hat x_{ms}=\int xp(x|z)dx:=E\left \{ x|z\right \}(9)
x^ms=∫xp(x∣z)dx:=E{x∣z}(9)
式(9)非常重要!!通过式(9)，我们可以发现，在最小均方误差准则下，估计值
x
^
\hat x
x^的最优解就是后验概率密度函数的一阶矩。而我们知道，卡尔曼滤波器正是基于最小均方误差准则的，这就是为什么在许多谈及卡尔曼滤波器的论文中，都会说卡尔曼滤波器就是在传递后验概率密度函数的一阶矩的原因！！如下图所示
由于估计值是基于最小均方误差准则下，对随机变量的估计(因为存在噪声)，我们希望可以评估这些估计值的不确定性。在这里，我们使用误差协方差矩阵来作为评估标准
P
x
x
=
E
{
(
x
−
x
^
)
(
x
−
x
^
)
T
∣
z
}
=
∫
(
x
−
x
^
)
(
x
−
x
^
)
T
p
(
x
∣
z
)
d
x
(
10
)
P^{xx}=E\left \{ (x- \hat x)(x -\hat x)^{T}|z\right \}=\int (x- \hat x)(x -\hat x)^{T}p(x|z)dx(10)
Pxx=E{(x−x^)(x−x^)T∣z}=∫(x−x^)(x−x^)Tp(x∣z)dx(10)
3.概率密度函数的递归贝叶斯滤波
通过前两节的介绍，我们初步明白了，为什么在贝叶斯框架下的众多滤波算法，后验概率密度及其统计特性对于我们求解状态估计值是如此的重要。接下来，我们希望可以向着“得到实际工程可用的滤波公式”这一目标推进。
一般的估计问题，是在一个更现实的环境中，譬如是对一个连续运动的过程。连续运动过程的解很难在工程中使用，因此，我们会对该过程做离散化处理。
对于一个离散的动态过程来说，我们一般定义该过程当前的状态是基于上一个或是多个过去时刻的状态。在一个离散过程中，当前时刻
t
n
t_{n}
tn与过去时刻的依赖性可以由一个差分方程表示。其中，一阶马尔可夫过程指的是当前的状态仅依赖于上一个时刻的状态，该过程可以表示为
x
n
=
f
n
−
1
(
x
n
−
1
,
u
n
,
η
n
−
1
)
=
f
n
−
1
(
x
n
−
1
)
+
u
n
+
v
n
−
1
(
11
)
x_{n}=f_{n-1}(x_{n-1},u_{n},\eta_{n-1})=f_{n-1}(x_{n-1})+u_{n}+v_{n-1}(11)
xn=fn−1(xn−1,un,ηn−1)=fn−1(xn−1)+un+vn−1(11)
其中， x n x_{n} xn是指 t n t_{n} tn时刻的状态向量， f n − 1 f_{n-1} fn−1是指状态转移函数， u ( n ) u(n) u(n)是已知的控制向量， η ( n − 1 ) \eta(n-1) η(n−1)是指白噪声。需要注意的是， η ( n − 1 ) \eta (n-1) η(n−1)一般被认为是真实的转移函数中没有被建模地那一部分。举个例子来说，假如用上式来对一艘运动的游艇进行建模，该模型被建模为匀速运动模型，那么噪声那部分就是代表着由于游艇的加速运动所带来的偏差。
我们通常要根据所有可观察向量
z
1
:
n
:
=
{
z
1
,
z
2
,
.
.
.
,
z
n
}
z_{1:n}:=\left \{ z_{1},z_{2},...,z_{n}\right \}
z1:n:={z1,z2,...,zn}来估计不可观察的状态向量
x
n
x_{n}
xn。因为，我们需要作出假设(如果不作出假设，许多问题将无法得到解析解)。我们假设在
t
n
t_{n}
tn时刻的观测向量与
t
n
t_{n}
tn时刻的状态向量之间存在着解析联系，并将其表示为
z
n
=
h
n
(
x
n
,
η
n
)
=
h
n
(
x
n
)
+
w
n
(
12
)
z_{n}=h_{n}(x_{n},\eta_{n})=h_{n}(x_{n})+w_{n}(12)
zn=hn(xn,ηn)=hn(xn)+wn(12)
其中，
z
n
z_{n}
zn代表着观测向量，
h
n
h_{n}
hn为确定的观测函数，将
x
n
x_{n}
xn与
z
n
z_{n}
zn关联起来。同样，
η
(
n
−
1
)
\eta (n-1)
η(n−1)一般被认为是真实的转移函数中没有被建模地那一部分。
通常，对大部分问题来说，
f
n
f_{n}
fn与
h
n
h_{n}
hn被认为跟随时间变化的速度是非常慢的，可近似认为与时间无关，因此，记为
f
n
→
f
f_{n}\rightarrow f
fn→f,
h
n
→
h
h_{n}\rightarrow h
hn→h。
式(11)和式(12)可以看作估计问题中对一个系统的完整建模。由于我们现在需要基于完整的观测向量的 z 1 : n z_{1:n} z1:n来对 x n x_{n} xn做出估计，同时结合上一小节的结论，我们发现，在一阶卡尔可夫过程的假设下，估计问题转变为了对后验概率密度函数 p ( x n ∣ z 1 : n ) p(x_{n}|z_{1:n}) p(xn∣z1:n)的估计问题。
现在重写贝叶斯公式
p
(
x
n
∣
z
1
:
n
)
=
p
(
z
1
:
n
∣
x
n
)
p
(
x
n
)
p
(
z
1
:
n
)
(
13
)
p(x_{n}|z_{1:n})=\frac{p(z_{1:n}|x_{n})p(x_{n})}{p(z_{1:n})}(13)
p(xn∣z1:n)=p(z1:n)p(z1:n∣xn)p(xn)(13)
其中，后验概率密度函数
p
(
x
n
∣
z
1
:
n
)
p(x_{n}|z_{1:n})
p(xn∣z1:n)是在考虑了过去所有时刻，包括当前时刻的观测值情况下，变量
x
n
x_{n}
xn的概率密度函数。由于观测集合
{
z
1
:
n
}
\left \{ z_{1:n}\right \}
{z1:n}可以被写为
{
z
n
,
z
1
:
n
−
1
}
\left \{ z_{n},z_{1:n-1}\right \}
{zn,z1:n−1}，因此式(13)可以被写为
p
(
x
n
∣
z
1
:
n
)
=
p
(
z
n
,
z
1
:
n
−
1
∣
x
n
)
p
(
x
n
)
p
(
z
n
,
z
1
:
n
−
1
)
(
14
)
p(x_{n}|z_{1:n})=\frac{p(z_{n},z_{1:n-1}|x_{n})p(x_{n})}{p(z_{n},z_{1:n-1})}(14)
p(xn∣z1:n)=p(zn,z1:n−1)p(zn,z1:n−1∣xn)p(xn)(14)
p
(
x
n
∣
z
1
:
n
)
=
p
(
z
n
∣
z
1
:
n
−
1
,
x
n
)
p
(
z
1
:
n
−
1
∣
x
n
)
p
(
x
n
)
p
(
z
n
∣
z
1
:
n
−
1
)
p
(
z
1
:
n
−
1
)
(
15
)
p(x_{n}|z_{1:n})=\frac{p(z_{n}|z_{1:n-1},x_{n})p(z_{1:n-1}|x_{n})p(x_{n})}{p(z_{n}|z_{1:n-1})p(z_{1:n-1})}(15)
p(xn∣z1:n)=p(zn∣z1:n−1)p(z1:n−1)p(zn∣z1:n−1,xn)p(z1:n−1∣xn)p(xn)(15)
由式(12)可知，当前时刻的观测值
z
n
z_{n}
zn与过去时刻的观测值
z
1
:
n
−
1
z_{1:n-1}
z1:n−1也无关。式(15)可以进一步修正为
p
(
x
n
∣
z
1
:
n
)
=
p
(
z
n
∣
x
n
)
p
(
x
n
∣
z
1
:
n
−
1
)
p
(
z
n
∣
z
1
:
n
−
1
)
(
16
)
p(x_{n}|z_{1:n})=\frac{p(z_{n}|x_{n})p(x_{n}|z_{1:n-1})}{p(z_{n}|z_{1:n-1})}(16)
p(xn∣z1:n)=p(zn∣z1:n−1)p(zn∣xn)p(xn∣z1:n−1)(16)
注意，本节的目标是得出理想的概率密度函数的完整递归形式。现在我们已经得到由
p
(
x
n
∣
z
1
:
n
−
1
)
p(x_{n}|z_{1:n-1})
p(xn∣z1:n−1)求解
p
(
x
n
∣
z
1
:
n
)
p(x_{n}|z_{1:n})
p(xn∣z1:n)的公式。下一步，我们利用Chapman-Kolmogorov公式，来写出先验密度
p
(
x
n
∣
z
1
:
n
−
1
)
p(x_{n}|z_{1:n-1})
p(xn∣z1:n−1)与上一时刻的后验密度的关系式
p
(
x
n
∣
z
1
:
n
−
1
)
=
∫
p
(
x
n
∣
x
n
−
1
,
z
1
:
n
−
1
)
p
(
x
n
−
1
∣
z
1
:
n
−
1
)
d
x
n
−
1
=
∫
p
(
x
n
∣
x
n
−
1
)
p
(
x
n
−
1
∣
z
1
:
n
−
1
)
d
x
n
−
1
(
17
)
p(x_{n}|z_{1:n-1})=\int p(x_{n}|x_{n-1},z_{1:n-1})p(x_{n-1}|z_{1:n-1})dx_{n-1} \\=\int p(x_{n}|x_{n-1})p(x_{n-1}|z_{1:n-1})dx_{n-1}(17)
p(xn∣z1:n−1)=∫p(xn∣xn−1,z1:n−1)p(xn−1∣z1:n−1)dxn−1=∫p(xn∣xn−1)p(xn−1∣z1:n−1)dxn−1(17)
上式中，
p
(
x
n
∣
x
n
−
1
,
z
1
:
n
−
1
)
→
p
(
x
n
∣
x
n
−
1
)
p(x_{n}|x_{n-1},z_{1:n-1}) \rightarrow p(x_{n}|x_{n-1})
p(xn∣xn−1,z1:n−1)→p(xn∣xn−1),是因为，由式(11)知道，
x
n
x_{n}
xn不依赖于
z
1
:
n
−
1
z_{1:n-1}
z1:n−1。现在，结合式(16)与式(17)，我们就得到了上一时刻的后验概率密度函数
p
(
x
n
∣
z
1
:
n
−
1
)
p(x_{n}|z_{1:n-1})
p(xn∣z1:n−1)与当前时刻的后验概率密度函数
p
(
x
n
∣
z
1
:
n
)
p(x_{n}|z_{1:n})
p(xn∣z1:n)的完整递归关系！ 整理为，
{
p
(
x
n
∣
z
1
:
n
−
1
)
=
∫
p
(
x
n
∣
x
n
−
1
)
p
(
x
n
−
1
∣
z
1
:
n
−
1
)
d
x
n
−
1
p
(
x
n
∣
z
1
:
n
)
=
p
(
z
n
∣
x
n
)
p
(
x
n
∣
z
1
:
n
−
1
)
p
(
z
n
∣
z
1
:
n
−
1
)
(
18
)
\left\{\begin{matrix} p(x_{n}|z_{1:n-1})=\int p(x_{n}|x_{n-1})p(x_{n-1}|z_{1:n-1})dx_{n-1}\\ p(x_{n}|z_{1:n})=\frac{p(z_{n}|x_{n})p(x_{n}|z_{1:n-1})}{p(z_{n}|z_{1:n-1})} \end{matrix}\right.(18)
{p(xn∣z1:n−1)=∫p(xn∣xn−1)p(xn−1∣z1:n−1)dxn−1p(xn∣z1:n)=p(zn∣z1:n−1)p(zn∣xn)p(xn∣z1:n−1)(18)
引用书中的图例，来说明递归贝叶斯后验概率估计过程。
4.递归贝叶斯估计的状态均值与协方差
由第二小节 点估计 的分析可知，在最小均方误差准则下，估计值 x ^ \hat x x^的最优解就是后验概率密度函数的一阶矩。
由第三小节的分析可知，后验概率的递归求解大致可分为两步，即预测， p ( x n − 1 ∣ z 1 : n − 1 ) → p ( x n ∣ z 1 : n − 1 ) p(x_{n-1}|z_{1:n-1}) \rightarrow p(x_{n}|z_{1:n-1}) p(xn−1∣z1:n−1)→p(xn∣z1:n−1),和更新， p ( x n ∣ z 1 : n − 1 ) → p ( x n ∣ z 1 : n ) p(x_{n}|z_{1:n-1}) \rightarrow p(x_{n}|z_{1:n}) p(xn∣z1:n−1)→p(xn∣z1:n)。
因此，我们先列出以下几个等式。基于过去所有时刻的观测值，包括当前时刻观测值，对
t
n
t_{n}
tn时刻的
x
n
x_{n}
xn的估计值可以记为
x
^
n
∣
n
=
E
{
x
n
∣
z
1
:
n
}
=
∫
x
n
p
(
x
n
∣
z
1
:
n
)
d
x
n
(
19
)
\hat x_{n|n}=E\left \{ x_{n}|z_{1:n}\right \}=\int x_{n}p(x_{n}|z_{1:n})dx_{n}(19)
x^n∣n=E{xn∣z1:n}=∫xnp(xn∣z1:n)dxn(19)
基于过去所有时刻的观测值，不包括当前时刻观测值，对
t
n
t_{n}
tn时刻的
x
n
x_{n}
xn的预测值可以记为
x
^
n
∣
n
−
1
=
E
{
x
n
∣
z
1
:
n
−
1
}
=
∫
x
n
p
(
x
n
∣
z
1
:
n
−
1
)
d
x
n
(
20
)
\hat x_{n|n-1}=E\left \{ x_{n}|z_{1:n-1}\right \}=\int x_{n}p(x_{n}|z_{1:n-1})dx_{n}(20)
x^n∣n−1=E{xn∣z1:n−1}=∫xnp(xn∣z1:n−1)dxn(20)
同样，估计的误差协方差可以写为
P
n
∣
n
x
x
=
E
{
(
x
n
−
x
^
n
∣
n
)
(
x
n
−
x
^
n
∣
n
)
T
∣
z
1
:
n
}
=
∫
(
x
n
−
x
^
n
∣
n
)
(
x
n
−
x
^
n
∣
n
)
T
p
(
x
n
∣
z
1
:
n
)
d
x
n
(
21
)
P^{xx}_{n|n}=E\left \{ (x_{n}-\hat x_{n|n})(x_{n}-\hat x_{n|n})^{T}|z_{1:n}\right \} \\=\int (x_{n}-\hat x_{n|n})(x_{n}-\hat x_{n|n})^{T}p(x_{n}|z_{1:n})dx_{n} (21)
Pn∣nxx=E{(xn−x^n∣n)(xn−x^n∣n)T∣z1:n}=∫(xn−x^n∣n)(xn−x^n∣n)Tp(xn∣z1:n)dxn(21)
预测的协方差可以写为
P
n
∣
n
−
1
x
x
=
E
{
(
x
n
−
x
^
n
∣
n
−
1
)
(
x
n
−
x
^
n
∣
n
−
1
)
T
∣
z
1
:
n
−
1
}
=
∫
(
x
n
−
x
^
n
∣
n
−
1
)
(
x
n
−
x
^
n
∣
n
−
1
)
T
p
(
x
n
∣
z
1
:
n
−
1
)
d
x
n
(
22
)
P^{xx}_{n|n-1}=E\left \{ (x_{n}-\hat x_{n|n-1})(x_{n}-\hat x_{n|n-1})^{T}|z_{1:n-1}\right \} \\=\int (x_{n}-\hat x_{n|n-1})(x_{n}-\hat x_{n|n-1})^{T}p(x_{n}|z_{1:n-1})dx_{n} (22)
Pn∣n−1xx=E{(xn−x^n∣n−1)(xn−x^n∣n−1)T∣z1:n−1}=∫(xn−x^n∣n−1)(xn−x^n∣n−1)Tp(xn∣z1:n−1)dxn(22)
4.1 状态向量的预测
现在，我们将之前所作的假设都代入上述公式中
使用Chapman-Kolmogorov公式(17)，并带入式(20)，可以得到
x
^
n
∣
n
−
1
=
∫
∫
x
n
p
(
x
n
∣
x
n
−
1
)
p
(
x
n
−
1
∣
z
1
:
n
−
1
)
d
x
n
−
1
d
x
n
(
23
)
\hat x_{n|n-1}=\int \int x_{n}p(x_{n}|x_{n-1})p(x_{n-1}|z_{1:n-1})dx_{n-1}dx_{n}(23)
x^n∣n−1=∫∫xnp(xn∣xn−1)p(xn−1∣z1:n−1)dxn−1dxn(23)
将一阶马尔可夫过程假设下得到的状态方程(11)代入上式，可以得到
x
^
n
∣
n
−
1
=
∫
∫
[
f
n
−
1
(
x
n
−
1
)
+
u
n
+
v
n
−
1
]
p
(
x
n
∣
x
n
−
1
)
p
(
x
n
−
1
∣
z
1
:
n
−
1
)
d
x
n
−
1
d
x
n
=
∫
[
f
n
−
1
(
x
n
−
1
)
+
u
n
+
v
n
−
1
]
p
(
x
n
−
1
∣
z
1
:
n
−
1
)
d
x
n
−
1
∫
p
(
x
n
∣
x
n
−
1
)
d
x
n
=
∫
f
n
−
1
(
x
n
−
1
)
p
(
x
n
−
1
∣
z
1
:
n
−
1
)
d
x
n
−
1
+
u
n
+
∫
v
n
−
1
p
(
x
n
−
1
∣
z
1
:
n
−
1
)
d
x
n
−
1
(
24
)
\hat x_{n|n-1}=\int \int \left [f_{n-1}(x_{n-1})+u_{n}+v_{n-1}\right ]p(x_{n}|x_{n-1})p(x_{n-1}|z_{1:n-1})dx_{n-1}dx_{n} \\=\int \left [f_{n-1}(x_{n-1})+u_{n}+v_{n-1}\right ]p(x_{n-1}|z_{1:n-1})dx_{n-1}\int p(x_{n}|x_{n-1})dx_{n} \\= \int f_{n-1}(x_{n-1})p(x_{n-1}|z_{1:n-1})dx_{n-1}+u_{n}+\int v_{n-1}p(x_{n-1}|z_{1:n-1})dx_{n-1}(24)
x^n∣n−1=∫∫[fn−1(xn−1)+un+vn−1]p(xn∣xn−1)p(xn−1∣z1:n−1)dxn−1dxn=∫[fn−1(xn−1)+un+vn−1]p(xn−1∣z1:n−1)dxn−1∫p(xn∣xn−1)dxn=∫fn−1(xn−1)p(xn−1∣z1:n−1)dxn−1+un+∫vn−1p(xn−1∣z1:n−1)dxn−1(24)
同样，使用Chapman-Kolmogorov公式(17)，并带入式(22)，得到
P
n
∣
n
−
1
x
x
=
∫
∫
(
x
n
−
x
^
n
∣
n
−
1
)
(
x
n
−
x
^
n
∣
n
−
1
)
T
p
(
x
n
∣
x
n
−
1
)
p
(
x
n
−
1
∣
z
1
:
n
−
1
)
d
x
n
−
1
d
x
n
(
25
)
P^{xx}_{n|n-1}=\int\int (x_{n}-\hat x_{n|n-1})(x_{n}-\hat x_{n|n-1})^{T}p(x_{n}|x_{n-1})p(x_{n-1}|z_{1:n-1})dx_{n-1}dx_{n}(25)
Pn∣n−1xx=∫∫(xn−x^n∣n−1)(xn−x^n∣n−1)Tp(xn∣xn−1)p(xn−1∣z1:n−1)dxn−1dxn(25)
将状态方程(11)代入上式，可以得到
P
n
∣
n
−
1
x
x
=
∫
[
f
n
−
1
(
x
n
−
1
)
+
u
n
−
x
^
n
∣
n
−
1
]
[
f
n
−
1
(
x
n
−
1
)
+
u
n
−
x
^
n
∣
n
−
1
]
T
p
(
x
n
−
1
∣
z
1
:
n
−
1
)
d
x
n
−
1
−
∫
[
f
n
−
1
(
x
n
−
1
)
+
u
n
−
x
^
n
∣
n
−
1
]
v
n
−
1
T
p
(
x
n
−
1
∣
z
1
:
n
−
1
)
d
x
n
−
1
−
∫
v
n
−
1
[
f
n
−
1
(
x
n
−
1
)
+
u
n
−
x
^
n
∣
n
−
1
]
T
p
(
x
n
−
1
∣
z
1
:
n
−
1
)
d
x
n
−
1
+
∫
v
n
−
1
v
n
−
1
T
p
(
x
n
−
1
∣
z
1
:
n
−
1
)
d
x
n
−
1
(
26
)
P^{xx}_{n|n-1}=\int \left [f_{n-1}(x_{n-1})+u_{n}-\hat x_{n|n-1}\right ]\left [f_{n-1}(x_{n-1})+u_{n}-\hat x_{n|n-1}\right ]^{T}p(x_{n-1}|z_{1:n-1})dx_{n-1} \\-\int \left [f_{n-1}(x_{n-1})+u_{n}-\hat x_{n|n-1}\right ]v^{T}_{n-1}p(x_{n-1}|z_{1:n-1})dx_{n-1}\\- \int v_{n-1} \left [f_{n-1}(x_{n-1})+u_{n}-\hat x_{n|n-1}\right ]^{T}p(x_{n-1}|z_{1:n-1})dx_{n-1}\\+\int v_{n-1}v_{n-1}^{T}p(x_{n-1}|z_{1:n-1})dx_{n-1}(26)
Pn∣n−1xx=∫[fn−1(xn−1)+un−x^n∣n−1][fn−1(xn−1)+un−x^n∣n−1]Tp(xn−1∣z1:n−1)dxn−1−∫[fn−1(xn−1)+un−x^n∣n−1]vn−1Tp(xn−1∣z1:n−1)dxn−1−∫vn−1[fn−1(xn−1)+un−x^n∣n−1]Tp(xn−1∣z1:n−1)dxn−1+∫vn−1vn−1Tp(xn−1∣z1:n−1)dxn−1(26)
定义噪声协方差为
Q
:
=
∫
v
n
−
1
v
n
−
1
T
p
(
x
n
−
1
∣
z
1
:
n
−
1
)
d
x
n
−
1
(
27
)
Q:=\int v_{n-1}v_{n-1}^{T}p(x_{n-1}|z_{1:n-1})dx_{n-1}(27)
Q:=∫vn−1vn−1Tp(xn−1∣z1:n−1)dxn−1(27)
并且假设
f
n
−
1
(
x
n
−
1
,
u
n
)
f_{n-1}(x_{n-1},u_{n})
fn−1(xn−1,un)与
v
n
−
1
v_{n-1}
vn−1无关，则上式中，中间两个积分的值为0，最后整理得
P
n
∣
n
−
1
x
x
=
∫
[
f
n
−
1
(
x
n
−
1
)
+
u
n
−
x
^
n
∣
n
−
1
]
[
f
n
−
1
(
x
n
−
1
)
+
u
n
−
x
^
n
∣
n
−
1
]
T
p
(
x
n
−
1
∣
z
1
:
n
−
1
)
d
x
n
−
1
+
Q
(
28
)
P^{xx}_{n|n-1}=\int \left [f_{n-1}(x_{n-1})+u_{n}-\hat x_{n|n-1}\right ]\left [f_{n-1}(x_{n-1})+u_{n}-\hat x_{n|n-1}\right ]^{T}p(x_{n-1}|z_{1:n-1})dx_{n-1}\\ +Q(28)
Pn∣n−1xx=∫[fn−1(xn−1)+un−x^n∣n−1][fn−1(xn−1)+un−x^n∣n−1]Tp(xn−1∣z1:n−1)dxn−1+Q(28)
4.2 状态向量的更新
对于一般密度而言，卡尔曼提出了一种不使用贝叶斯准则的滤波器。卡尔曼假设只需要更新一阶矩与二阶矩就可以估计系统的状态
x
n
x_{n}
xn。同时，卡尔曼假设当前时刻对一阶矩的更新估计是线性的，并且依赖于当前观测，表示为
x
^
n
∣
n
=
A
z
n
0
+
b
(
29
)
\hat x_{n|n}=Az_{n}^{0}+b(29)
x^n∣n=Azn0+b(29)
其中，
z
n
0
z_{n}^{0}
zn0是当前时刻带噪声的观测值，而
A
,
b
A,b
A,b未知。
我们知道，基于观测值 z n 0 z_{n}^{0} zn0,对随机变量 x n x_{n} xn的最优线性估计满足以下两个条件：
1.条件估计是无偏的，即估计误差的均值为0，表示为 E { ϵ n x ∣ z 1 : n − 1 } = 0 E\left \{ \epsilon_{n}^{x}|z_{1:n-1} \right \}=0 E{ϵnx∣z1:n−1}=0
2.估计误差与观测值无关，表示为 E { ϵ n x ( z n 0 ) T ∣ z 1 : n − 1 } = 0 E\left \{ \epsilon_{n}^{x}(z_{n}^{0})^{T}|z_{1:n-1}\right \}=0 E{ϵnx(zn0)T∣z1:n−1}=0
将估计误差的定义式以及公式(29)带入条件1，得到
E
{
ϵ
n
x
∣
z
1
:
n
−
1
}
=
E
{
x
n
−
x
^
n
∣
n
∣
z
1
:
n
−
1
}
=
E
{
x
n
∣
z
1
:
n
−
1
}
−
(
A
E
{
z
n
0
∣
z
1
:
n
−
1
}
+
b
)
=
x
^
n
∣
n
−
1
−
A
z
^
n
∣
n
−
1
−
b
=
0
(
30
)
E\left \{ \epsilon_{n}^{x}|z_{1:n-1} \right \}=E\left \{ x_{n}-\hat x_{n|n}|z_{1:n-1} \right \}\\=E\left \{ x_{n}|z_{1:n-1} \right \}-(AE\left \{ z_{n}^{0}|z_{1:n-1}\right \}+b)\\=\hat x_{n|n-1}-A \hat z_{n|n-1}-b=0(30)
E{ϵnx∣z1:n−1}=E{xn−x^n∣n∣z1:n−1}=E{xn∣z1:n−1}−(AE{zn0∣z1:n−1}+b)=x^n∣n−1−Az^n∣n−1−b=0(30)
因此，
b
=
x
^
n
∣
n
−
1
−
A
z
^
n
∣
n
−
1
(
31
)
b=\hat x_{n|n-1}-A \hat z_{n|n-1}(31)
b=x^n∣n−1−Az^n∣n−1(31)
将上式带入式(29)，可以得到
x
^
n
∣
n
=
x
^
n
∣
n
−
1
+
A
(
z
n
0
−
z
^
n
∣
n
−
1
)
(
32
)
\hat x_{n|n}=\hat x_{n|n-1}+A(z_{n}^{0}-\hat z_{n|n-1})(32)
x^n∣n=x^n∣n−1+A(zn0−z^n∣n−1)(32)
(是不是很眼熟？)
再考虑条件2。条件2下，估计误差与观测值正交，
E
{
ϵ
n
x
(
z
n
0
)
T
∣
z
1
:
n
−
1
}
=
E
{
[
x
n
−
x
^
n
∣
n
−
1
−
A
(
z
n
0
−
z
^
n
∣
n
−
1
)
]
(
z
n
0
)
T
∣
z
1
:
n
−
1
}
(
33
)
E\left \{ \epsilon_{n}^{x}(z_{n}^{0})^{T}|z_{1:n-1}\right \}=E\left \{ [x_{n}-\hat x_{n|n-1}-A(z_{n}^{0}-\hat z_{n|n-1})](z_{n}^{0})^{T}|z_{1:n-1}\right \}(33)
E{ϵnx(zn0)T∣z1:n−1}=E{[xn−x^n∣n−1−A(zn0−z^n∣n−1)](zn0)T∣z1:n−1}(33)
由于
E
{
ϵ
n
x
]
}
=
0
E\left \{ \epsilon_{n}^{x]} \right \}=0
E{ϵnx]}=0,则
E
{
ϵ
n
x
(
z
n
0
)
T
}
=
E
{
ϵ
n
x
(
z
n
0
−
z
^
n
∣
n
−
1
)
T
}
E\left \{ \epsilon_{n}^{x}(z_{n}^{0})^{T}\right \}=E\left \{ \epsilon_{n}^{x}(z_{n}^{0}-\hat z_{n|n-1})^{T}\right \}
E{ϵnx(zn0)T}=E{ϵnx(zn0−z^n∣n−1)T},而式(33)可以改写为
E
{
ϵ
n
x
(
z
n
0
)
T
∣
z
1
:
n
−
1
}
=
E
{
[
x
n
−
x
^
n
∣
n
−
1
−
A
(
z
n
0
−
z
^
n
∣
n
−
1
)
]
(
z
n
0
−
z
^
n
∣
n
−
1
)
T
∣
z
1
:
n
−
1
}
=
P
n
∣
n
−
1
x
z
−
A
P
n
∣
n
−
1
z
z
=
0
(
34
)
E\left \{ \epsilon_{n}^{x}(z_{n}^{0})^{T}|z_{1:n-1}\right \}=E\left \{ [x_{n}-\hat x_{n|n-1}-A(z_{n}^{0}-\hat z_{n|n-1})](z_{n}^{0}-\hat z_{n|n-1})^{T}|z_{1:n-1}\right \}\\=P^{xz}_{n|n-1}-AP^{zz}_{n|n-1}=0(34)
E{ϵnx(zn0)T∣z1:n−1}=E{[xn−x^n∣n−1−A(zn0−z^n∣n−1)](zn0−z^n∣n−1)T∣z1:n−1}=Pn∣n−1xz−APn∣n−1zz=0(34)
则
A
A
A可表示为
A
=
K
n
:
=
P
n
∣
n
−
1
x
z
[
P
n
∣
n
−
1
z
z
]
−
1
(
35
)
A=K_{n}:=P_{n|n-1}^{xz}\left [ P_{n|n-1}^{zz} \right ]^{-1}(35)
A=Kn:=Pn∣n−1xz[Pn∣n−1zz]−1(35)
整理上述公式，可以得到在线性最小均方误差准则的估计器下，
x
^
n
∣
n
=
x
^
n
∣
n
−
1
+
K
n
(
z
n
0
−
z
^
n
∣
n
−
1
)
(
36
)
\hat x_{n|n}=\hat x_{n|n-1}+K_{n}(z_{n}^{0}-\hat z_{n|n-1})(36)
x^n∣n=x^n∣n−1+Kn(zn0−z^n∣n−1)(36)
定义新息
ϵ
n
x
:
=
z
n
0
−
z
^
n
∣
n
−
1
(
37
)
\epsilon_{n}^{x}:=z_{n}^{0}-\hat z_{n|n-1}(37)
ϵnx:=zn0−z^n∣n−1(37)
更新误差协方差公式可以由下式得到
P
n
∣
n
x
x
=
E
{
ϵ
n
x
(
ϵ
n
x
)
T
∣
z
1
:
n
−
1
}
=
P
n
∣
n
−
1
x
x
−
K
n
P
n
∣
n
−
1
z
z
K
n
T
(
38
)
P^{xx}_{n|n}=E\left \{ \epsilon_{n}^{x} (\epsilon_{n}^{x})^{T}|z_{1:n-1}\right \}\\=P_{n|n-1}^{xx}-K_{n}P^{zz}_{n|n-1}K_{n}^{T}(38)
Pn∣nxx=E{ϵnx(ϵnx)T∣z1:n−1}=Pn∣n−1xx−KnPn∣n−1zzKnT(38)
至此，结合式(35)、式(36)、式(38)，我们已经初步得到了卡尔曼滤波器更新公式的雏形。只要使用最新的观测值，就能对式(24)、式(28)得到的预测值做更新估计。然而，观察公式发现，想要在实际问题中使用，我们还需要求得
z
^
n
∣
n
−
1
,
P
n
∣
n
−
1
z
z
,
P
n
∣
n
−
1
x
z
\hat z_{n|n-1},P_{n|n-1}^{zz},P_{n|n-1}^{xz}
z^n∣n−1,Pn∣n−1zz,Pn∣n−1xz。
接下来对
z
^
n
∣
n
−
1
,
P
n
∣
n
−
1
z
z
,
P
n
∣
n
−
1
x
z
\hat z_{n|n-1},P_{n|n-1}^{zz},P_{n|n-1}^{xz}
z^n∣n−1,Pn∣n−1zz,Pn∣n−1xz的公式作进一步推导。
z
^
n
∣
n
−
1
=
E
{
z
n
∣
x
n
,
z
1
:
n
−
1
}
=
∫
h
n
(
x
n
)
p
(
x
n
∣
z
1
:
n
−
1
)
d
x
n
+
∫
w
n
p
(
x
n
∣
z
1
:
n
−
1
)
d
x
n
(
39
)
\hat z_{n|n-1}=E\left \{ z_{n}|x_{n},z_{1:n-1}\right \}\\=\int h_{n}(x_{n})p(x_{n}|z_{1:n-1})dx_{n}\\+\int w_{n}p(x_{n}|z_{1:n-1})dx_{n}(39)
z^n∣n−1=E{zn∣xn,z1:n−1}=∫hn(xn)p(xn∣z1:n−1)dxn+∫wnp(xn∣z1:n−1)dxn(39)
P n ∣ n − 1 z z = E { ( z n 0 − z ^ n ∣ n − 1 ) ( z n 0 − z ^ n ∣ n − 1 ) T ∣ x n , z 1 : n − 1 } = ∫ [ h n ( x n ) − z ^ n ∣ n − 1 ] [ h n ( x n ) − z ^ n ∣ n − 1 ] T × p ( x n ∣ z 1 : n − 1 ) d x n + R ( 40 ) P^{zz}_{n|n-1}=E\left \{ (z_{n}^{0}-\hat z_{n|n-1})(z_{n}^{0}-\hat z_{n|n-1})^{T}|x_{n},z_{1:n-1}\right \}\\=\int [h_{n}(x_{n})-\hat z_{n|n-1}][h_{n}(x_{n})-\hat z_{n|n-1}]^{T}\\ \times p(x_{n}|z_{1:n-1})dx_{n}+R(40) Pn∣n−1zz=E{(zn0−z^n∣n−1)(zn0−z^n∣n−1)T∣xn,z1:n−1}=∫[hn(xn)−z^n∣n−1][hn(xn)−z^n∣n−1]T×p(xn∣z1:n−1)dxn+R(40)
P n ∣ n − 1 x z = E { ( x n − x ^ n ∣ n − 1 ) ( z n 0 − z ^ n ∣ n − 1 ) T ∣ x n , z 1 : n − 1 } = ∫ [ x n − x ^ n ∣ n − 1 ] [ h n ( x n ) − z ^ n ∣ n − 1 ] T × p ( x n ∣ z 1 : n − 1 ) d x n ( 41 ) P^{xz}_{n|n-1}=E\left \{ (x_{n}-\hat x_{n|n-1})(z_{n}^{0}-\hat z_{n|n-1})^{T}|x_{n},z_{1:n-1}\right \}\\=\int [x_{n}-\hat x_{n|n-1}][h_{n}(x_{n})-\hat z_{n|n-1}]^{T}\\ \times p(x_{n}|z_{1:n-1})dx_{n}(41) Pn∣n−1xz=E{(xn−x^n∣n−1)(zn0−z^n∣n−1)T∣xn,z1:n−1}=∫[xn−x^n∣n−1][hn(xn)−z^n∣n−1]T×p(xn∣z1:n−1)dxn(41)
其中， R : = ∫ w n w n T p ( x n ∣ z 1 : n − 1 ) d x n R:=\int w_{n}w_{n}^{T}p(x_{n}|z_{1:n-1})dx_{n} R:=∫wnwnTp(xn∣z1:n−1)dxn。
现在做一个小总结，我们在第一小节回顾了贝叶斯公式，在第二小节推导出了在最小均方误差估计器下，如何运用后验概率密度 p ( x ∣ z ) p(x|z) p(x∣z)求得最优估计值 x ^ \hat x x^,揭示了后验概率密度函数 p ( x ∣ z ) p(x|z) p(x∣z)与最优估计值 x ^ \hat x x^的关系式。在第三小节，我们考虑了更实际的情况，在离散过程中使用贝叶斯公式，得到了一阶马尔可夫过程下，后验概率密度 p ( x n ∣ z 1 : n ) p(x_{n}|z_{1:n}) p(xn∣z1:n)如何在相邻周期间不断迭代（因为我们需要用后验概率密度函数去求解最优估计值），即后验概率密度 p ( x n ∣ z 1 : n ) p(x_{n}|z_{1:n}) p(xn∣z1:n)与后验概率密度 p ( x n − 1 ∣ z 1 : n − 1 ) p(x_{n-1}|z_{1:n-1}) p(xn−1∣z1:n−1)的关系式，并且发现，可以将估计问题分为预测与更新两个步骤。在第四小节，我们在一阶马尔可夫过程的假设下，推导了均值与协方差的预测与更新过程(因为均值代表着我们的最优估计值)，并在更新步骤中，按照卡尔曼提出的滤波器思想，在最小均方误差准则以及线性假设下，初步推导出了卡尔曼滤波器更新步骤的公式。
但是，目前所得到的公式，仍有多维积分以及参数未知等问题，并非解析解，离实际使用还有距离。
接下来请移步至 从贝叶斯估计到卡尔曼滤波(详细推导)(下)
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
