前言
卡尔曼滤波,动态模型里面比较具有代表性的一个模型。其和隐马尔可夫模型具有差不多相同的结构,但隐马尔可夫模型要求隐状态必须时离散的。而卡尔曼滤波则没有这个限制。本文将从贝叶斯派的角度去推导Filter问题。如果你没听过马尔可夫链,可以先去了解一下,否则估计很难看懂。或者尝试先去看看案例实现卡尔曼滤波运动轨迹优化
数学基础:【概率论与数理统计知识复习-哔哩哔哩】
引入
具体流程图就这样了,和隐马尔可夫模型都差不多。比如从时刻1开始,有隐状态z1,观测到状态x1。并且下一个状态可以通过 A ∗ z 1 + B A*z1+B A∗z1+B得到,而观测状态x通过 C ∗ z 1 + D C*z1+D C∗z1+D得到。
比如你可以将其用在小车运动轨迹的案例中的话,假如在时刻1,z1表示小车的的位置为0,而观测到的位置可以通过x1=Cz1+D计算。假设C=1,D=1,那么为 x 1 = 1 x1=1 x1=1。再到时刻2,可以通过时刻1的z1,计算z2.关系是z2=Az1+B。假如A=1,B=2。那么就可以计算出z2=2。再计算出x2…一直到最后
我们的目标就是求出 P ( z t ∣ x 1 , ⋅ , x t ) P(z_t|x_1,\cdot,x_t) P(zt∣x1,⋅,xt)。就是给定观测位置的情况下,综合它们的关系式,如何去求这个概率的参数。
推导
先对变量作一下定义
Z
=
(
z
1
z
2
⋮
z
T
)
;
X
=
(
x
1
x
2
⋮
x
T
)
Z=\begin{pmatrix}z_1 \\z_2 \\ \vdots \\ z_T \end{pmatrix}; X=\begin{pmatrix}x_1 \\x_2 \\ \vdots \\ x_T \end{pmatrix}
Z=
z1z2⋮zT
;X=
x1x2⋮xT
其中,
Z
Z
Z代表隐序列,
X
X
X代表观测序列。
为了简化运算和可行性,卡尔曼滤波仍然需要与隐马尔可夫模型的两个独立性假设。
①齐次马尔可夫假设:当前隐状态只和前一时刻的隐状态有关,其余都无关。数学表达:
P
(
z
t
∣
z
1
,
z
2
,
⋯
,
z
t
−
1
,
x
1
,
⋯
,
x
t
−
1
)
=
P
(
z
t
∣
z
t
−
1
)
P(z_t|z_1,z_2,\cdots,z_{t-1},x_1,\cdots,x_{t-1})=P(z_t|z_{t-1})
P(zt∣z1,z2,⋯,zt−1,x1,⋯,xt−1)=P(zt∣zt−1)
②观测独立性假设:当前观测状态仅和当前的隐状态有关,其余都无关。数学表达:
P
(
x
t
∣
x
1
,
x
2
,
⋯
,
x
t
−
1
,
z
1
,
⋯
,
z
t
)
=
P
(
x
t
∣
z
t
)
P(x_t|x_1,x_2,\cdots,x_{t-1},z_1,\cdots,z_{t})=P(x_t|z_{t})
P(xt∣x1,x2,⋯,xt−1,z1,⋯,zt)=P(xt∣zt)
除此以外,在卡尔曼滤波当中,其还有一个很重要的点就是,与隐马尔可夫模型中的状态转移概率和发射概率不同,其要求这两个概率必须是线性的,即(
z
t
,
x
t
,
z
t
−
1
,
u
,
v
z_t,x_t,z_{t-1},u,v
zt,xt,zt−1,u,v都是随机变量)
z
t
=
A
z
t
−
1
+
B
+
u
;
→
P
(
z
t
∣
z
t
−
1
)
x
t
=
C
z
t
+
D
+
v
;
→
P
(
x
t
∣
z
t
)
P
(
z
1
)
=
N
(
μ
0
,
Σ
0
)
;
z_t=Az_{t-1}+B+u;\rightarrow P(z_t|z_{t-1})\\ x_t=Cz_t+D+v;\rightarrow P(x_t|z_{t})\\ P(z_1)=N(\mu_0,\Sigma_0);
zt=Azt−1+B+u;→P(zt∣zt−1)xt=Czt+D+v;→P(xt∣zt)P(z1)=N(μ0,Σ0);
其中,
u
∼
N
(
0
,
Q
)
u\sim N(0,Q)
u∼N(0,Q),
v
∼
N
(
0
,
R
)
v\sim N(0,R)
v∼N(0,R),意为
u
u
u和
v
v
v都服从高斯分布,且协方差为
Q
Q
Q和
R
R
R。
并且, P ( z t ∣ z t − 1 ) ∼ N ( A z t − 1 + B , Q ) P(z_t|z_{t-1})\sim N(Az_{t-1}+B,Q) P(zt∣zt−1)∼N(Azt−1+B,Q), P ( x t ∣ z t ) ∼ N ( C z t + D , R ) P(x_t|z_{t})\sim N(Cz_t+D,R) P(xt∣zt)∼N(Czt+D,R),为什么会这样?感兴趣的可以参考线性动态系统中的概率求解-CSDN博客
所以参数 θ = { A , B , C , D , Q , R , μ 0 , Σ 0 } \theta=\left\{A,B,C,D,Q,R,\mu_0,Σ_0\right\} θ={A,B,C,D,Q,R,μ0,Σ0}。
在卡尔曼滤波中,我们暂且不讲参数如何估计的问题。主要针对其Filter问题进行推导
Filter
所谓求解Filter问题,实际上就是求解
P
(
z
t
∣
x
1
,
x
2
,
⋯
,
x
t
)
=
P
(
z
t
,
x
1
,
⋯
,
x
t
)
P
(
x
1
,
⋯
,
x
t
)
=
P
(
x
t
∣
z
t
,
x
1
,
⋯
,
x
t
−
1
)
P
(
z
t
,
x
1
,
⋯
,
x
t
−
1
)
P
(
x
1
,
⋯
,
x
t
)
=
P
(
x
t
∣
z
t
)
P
(
z
t
,
x
1
,
⋯
,
x
t
−
1
)
P
(
x
1
,
⋯
,
x
t
)
=
P
(
x
t
∣
z
t
)
P
(
z
t
∣
x
1
,
⋯
,
x
t
−
1
)
P
(
x
1
,
⋯
,
x
t
−
1
)
P
(
x
1
,
⋯
,
x
t
)
=
P
(
x
t
∣
z
t
)
P
(
z
t
∣
x
1
,
⋯
,
x
t
−
1
)
\begin{equation} \begin{aligned} P(z_t|x_1,x_2,\cdots,x_t)=&\frac{P(z_t,x_1,\cdots,x_t)}{P(x_1,\cdots,x_t)} \\=&\frac{P(x_t|z_t,x_1,\cdots,x_{t-1})P(z_t,x_1,\cdots,{x_{t-1}})}{P(x_1,\cdots,x_t)} \\=&\frac{P(x_t|z_t)P(z_t,x_1,\cdots,{x_{t-1}})}{P(x_1,\cdots,x_t)} \\=&\frac{P(x_t|z_t)P(z_t|x_1,\cdots,{x_{t-1}})P(x_1,\cdots,x_{t-1})}{P(x_1,\cdots,x_t)} \\=&P(x_t|z_t)P(z_t|x_1,\cdots,x_{t-1})\nonumber \end{aligned} \end{equation}
P(zt∣x1,x2,⋯,xt)=====P(x1,⋯,xt)P(zt,x1,⋯,xt)P(x1,⋯,xt)P(xt∣zt,x1,⋯,xt−1)P(zt,x1,⋯,xt−1)P(x1,⋯,xt)P(xt∣zt)P(zt,x1,⋯,xt−1)P(x1,⋯,xt)P(xt∣zt)P(zt∣x1,⋯,xt−1)P(x1,⋯,xt−1)P(xt∣zt)P(zt∣x1,⋯,xt−1)
对于第四和第五个等号,因为x是给定的,可以算出来的,所以可以暂时忽略(为什么可以忽略?因为我们只是想找到它的递推式,而不是要去计算它,并且后面我们采用的计算方法跟这个东西无关)
对里面的
P
(
z
t
∣
x
1
,
⋯
,
x
t
−
1
)
P(z_t|x_1,\cdots,x_{t-1})
P(zt∣x1,⋯,xt−1),一般叫做prediction
P
(
z
t
∣
x
1
,
⋯
,
x
t
−
1
)
=
∫
z
t
−
1
P
(
z
t
−
1
,
z
t
∣
x
1
,
⋯
,
x
t
−
1
)
d
z
t
−
1
=
∫
z
t
−
1
P
(
z
t
∣
z
t
−
1
,
x
1
,
⋯
,
x
t
−
1
)
P
(
z
t
−
1
∣
x
1
,
⋯
,
x
t
−
1
)
d
z
t
−
1
=
∫
z
t
−
1
P
(
z
t
∣
z
t
−
1
)
P
(
z
t
−
1
∣
x
1
,
⋯
,
x
t
−
1
)
d
z
t
−
1
\begin{equation} \begin{aligned} P(z_t|x_1,\cdots,x_{t-1})=&\int_{z_{t-1}}P(z_{t-1},z_t|x_1,\cdots,x_{t-1})d{z_{t-1}} \\=&\int_{z_{t-1}}P(z_t|z_{t-1},x_1,\cdots,x_{t-1})P(z_{t-1}|x_1,\cdots,x_{t-1})d{z_{t-1}} \\=&\int_{z_{t-1}}P(z_t|z_{t-1})P(z_{t-1}|x_1,\cdots,x_{t-1})d{z_{t-1}}\nonumber \end{aligned} \end{equation}
P(zt∣x1,⋯,xt−1)===∫zt−1P(zt−1,zt∣x1,⋯,xt−1)dzt−1∫zt−1P(zt∣zt−1,x1,⋯,xt−1)P(zt−1∣x1,⋯,xt−1)dzt−1∫zt−1P(zt∣zt−1)P(zt−1∣x1,⋯,xt−1)dzt−1
到这里你会发现 P ( z t − 1 ∣ x 1 , ⋯ , x t − 1 ) P(z_{t-1}|x_1,\cdots,x_{t-1}) P(zt−1∣x1,⋯,xt−1)就是上一个时刻的Filter,实际上其已经实现了递归式的求解方式。
在频率派中,他们更趋向于算出每一次的 P ( z i ∣ x 1 , ⋯ , x i ) P(z_i|x_1,\cdots,x_i) P(zi∣x1,⋯,xi),实际上,在贝叶斯派中,我们每一次我们都会算出前一个概率值,如果我们保留了所有的值,实际上跟频率派也是差不多的。
在应用的时候,实际上在一般的情况下,我们也是保留所有的值。比如在运动的位置预测当中。
所以,在卡尔曼滤波中,实际上分为两个部分
①预测:求出 P ( z t ∣ x 1 , ⋯ , x t − 1 ) P(z_t|x_1,\cdots,x_{t-1}) P(zt∣x1,⋯,xt−1)
②更新:求出 P ( z t ∣ x 1 , ⋯ , x t ) P(z_t|x_1,\cdots,x_{t}) P(zt∣x1,⋯,xt)
所以,现在的问题就变成了,如何求出这两个概率值。也就是求出他们的均值和协方差
在求解的时候,我们利用的是正太分布的一个特性:正态分布的相乘,相加,联合概率,条件概率等等操作后都仍然是正态分布。
在此之前,我们先对它们的参数做一下定义
对于 P ( z t ∣ x 1 , ⋯ , x t − 1 ) ∼ N ( μ ˉ t , Σ ˉ t ) P(z_t|x_1,\cdots,x_{t-1})\sim N(\bar\mu_t,\bar\Sigma_t) P(zt∣x1,⋯,xt−1)∼N(μˉt,Σˉt)
对于 P ( z t ∣ x 1 , ⋯ , x t ) ∼ N ( μ ^ t , Σ ^ t ) P(z_t|x_1,\cdots,x_{t})\sim N(\hat \mu_t,\hat \Sigma_t) P(zt∣x1,⋯,xt)∼N(μ^t,Σ^t)
求解
P ( z t ∣ x 1 , ⋯ , x t − 1 ) P(z_t|x_1,\cdots,x_{t-1}) P(zt∣x1,⋯,xt−1)
先来看一些公式
假设给定
{
P
(
x
)
∼
N
(
x
∣
μ
x
,
Σ
x
)
P
(
y
∣
x
)
∼
N
(
y
∣
A
x
+
B
,
Q
)
y
=
A
x
+
B
+
u
(1)
\left\{\begin{matrix} P(x)\sim N(x|\mu_x,\Sigma_x) \\P(y|x) \sim N(y|Ax+B,Q) \\y=Ax+B+u \end{matrix}\right.\tag{1}
⎩
⎨
⎧P(x)∼N(x∣μx,Σx)P(y∣x)∼N(y∣Ax+B,Q)y=Ax+B+u(1)
我们有
{
P
(
y
)
∼
N
(
y
∣
A
μ
x
+
B
,
A
Σ
x
A
T
+
Q
)
P
(
x
,
y
)
∼
N
(
x
,
y
∣
(
μ
x
A
μ
x
+
B
)
,
(
Σ
x
Σ
x
A
T
A
Σ
x
A
Σ
x
A
T
+
Q
)
)
(2)
\left\{\begin{matrix} P(y) \sim N(y|A\mu_x+B,A\Sigma_x A^T+Q) \\ P(x,y) \sim N\begin{pmatrix} \begin{matrix} x , y \end{matrix}| \begin{pmatrix} \mu_x \\ A\mu_x+B \end{pmatrix}, \begin{pmatrix} \Sigma_x &\Sigma_xA^T \\ A\Sigma_x & A\Sigma_x A^T+Q \end{pmatrix} \end{pmatrix} \end{matrix}\right.\tag{2}
⎩
⎨
⎧P(y)∼N(y∣Aμx+B,AΣxAT+Q)P(x,y)∼N(x,y∣(μxAμx+B),(ΣxAΣxΣxATAΣxAT+Q))(2)
至于这个东西怎么来的?感兴趣的话,可以去看看线性动态系统中的概率求解-CSDN博客
好,那么这个东西是什么东西呢?又有什么用呢?
我们先来看公式1,里面的意思是,对随机变量x,它服从期望为 μ x \mu_x μx,协方差矩阵为 Σ x \Sigma_x Σx的正态分布。而对于随机变量 y ∣ x y|x y∣x,它服从期望为 A x + B Ax+B Ax+B,协方差矩阵为 Q Q Q的正态分布。(N代表正态分布,|之后表示期望和协方差)
我们说,如果满足公式1的条件,则我们可以推出公式2。即
①对随机变量 y y y,满足 y = A x + B + u y=Ax+B+u y=Ax+B+u,其中 u ∼ N ( 0 , Q ) u \sim N(0,Q) u∼N(0,Q)
②对随机变量 y y y,有 y ∼ N ( A μ x + B , A Σ A T + Q ) y \sim N(A\mu_x+B,A\Sigma A^T+Q) y∼N(Aμx+B,AΣAT+Q)
③对于随机变量
x
,
y
x,y
x,y,有
x
,
y
∼
(
(
μ
x
A
μ
x
+
B
)
,
(
Σ
x
Σ
x
A
T
A
Σ
x
A
Σ
x
A
T
+
Q
)
)
x,y \sim\begin{pmatrix}\begin{pmatrix} \mu_x \\ A\mu_x+B \end{pmatrix}, \begin{pmatrix} \Sigma_x &\Sigma_xA^T \\ A\Sigma_x & A\Sigma_x A^T+Q \end{pmatrix}\end{pmatrix}
x,y∼((μxAμx+B),(ΣxAΣxΣxATAΣxAT+Q))
好,现在我们知道了有这些公式了,有什么用呢?
我们前面计算出来过
P
(
z
t
∣
x
1
,
⋯
,
x
t
−
1
)
=
∫
z
t
−
1
P
(
z
t
∣
z
t
−
1
,
x
1
,
⋯
,
x
t
−
1
)
P
(
z
t
−
1
∣
x
1
,
⋯
,
x
t
−
1
)
d
z
t
−
1
=
∫
z
t
−
1
P
(
z
t
∣
z
t
−
1
)
P
(
z
t
−
1
∣
x
1
,
⋯
,
x
t
−
1
)
d
z
t
−
1
\begin{equation} \begin{aligned} P(z_t|x_1,\cdots,x_{t-1})=&\int_{z_{t-1}}P(z_t|z_{t-1},x_1,\cdots,x_{t-1})P(z_{t-1}|x_1,\cdots,x_{t-1})d{z_{t-1}} \\=&\int_{z_{t-1}}P(z_t|z_{t-1})P(z_{t-1}|x_1,\cdots,x_{t-1})d{z_{t-1}}\nonumber \end{aligned} \end{equation}
P(zt∣x1,⋯,xt−1)==∫zt−1P(zt∣zt−1,x1,⋯,xt−1)P(zt−1∣x1,⋯,xt−1)dzt−1∫zt−1P(zt∣zt−1)P(zt−1∣x1,⋯,xt−1)dzt−1
来看
P
(
z
t
−
1
∣
x
1
,
⋯
,
x
t
−
1
)
P(z_{t-1}|x_1,\cdots,x_{t-1})
P(zt−1∣x1,⋯,xt−1),它的期望和协方差分别是
μ
^
t
−
1
,
Σ
^
t
−
1
\hat\mu_{t-1},\hat\Sigma_{t-1}
μ^t−1,Σ^t−1,并且,我们前面是给定
z
t
=
A
z
t
−
1
+
B
+
u
z_t=Az_{t-1}+B+u
zt=Azt−1+B+u
那么,如果我们给这个随机变量
z
t
z_t
zt变成条件随机变量,则会变成
z
t
∣
x
1
,
⋯
,
x
t
−
1
=
A
(
z
t
−
1
∣
x
1
,
⋯
,
x
t
−
1
)
+
B
+
u
(3)
z_t|x_1,\cdots,x_{t-1}=A(z_{t-1}|x_1,\cdots,x_{t-1})+B+u\tag{3}
zt∣x1,⋯,xt−1=A(zt−1∣x1,⋯,xt−1)+B+u(3)
好,现在,如果我们将
P
(
z
t
−
1
∣
x
1
,
⋯
,
x
t
−
1
)
P(z_{t-1}|x_1,\cdots,x_{t-1})
P(zt−1∣x1,⋯,xt−1)比作
P
(
x
)
P(x)
P(x)。将
∫
z
t
−
1
P
(
z
t
∣
z
t
−
1
,
x
1
,
⋯
,
x
t
−
1
)
\int_{z_{t-1}}P(z_t|z_{t-1},x_1,\cdots,x_{t-1})
∫zt−1P(zt∣zt−1,x1,⋯,xt−1)比作是
P
(
y
∣
x
)
P(y|x)
P(y∣x),依托公式3,则最终我们会发现,其刚好也符合我们上述所提到的公式1的条件。
那么,我们所要求的东西是什么?是 P ( z t ∣ x 1 , ⋯ , x t − 1 ) P(z_t|x_1,\cdots,x_{t-1}) P(zt∣x1,⋯,xt−1),这个东西是什么呢?没错,他可以比作 P ( y ) P(y) P(y),我只能讲到这样了(如果你对随机变量不太理解,你可以将条件随机变量 z t − 1 ∣ x 1 , ⋯ , x t − 1 z_{t-1}|x_1,\cdots,x_{t-1} zt−1∣x1,⋯,xt−1当作一个普通的随机变量,这样我相信你自然可以理解了)
既然如何,那么我们只需要求解出随机变量
y
y
y的参数就相当于求解出
P
(
z
t
∣
x
1
,
⋯
,
x
t
−
1
)
P(z_t|x_1,\cdots,x_{t-1})
P(zt∣x1,⋯,xt−1)所对应的参数。依据公式2,我们直接往里面代入,直接得到
μ
ˉ
t
=
A
μ
^
t
−
1
+
B
Σ
ˉ
t
=
A
Σ
^
t
−
1
A
T
+
Q
\begin{equation} \begin{aligned} \bar\mu_t=&A\hat\mu_{t-1}+B \\\bar\Sigma_t=&A\hat\Sigma_{t-1}A^T+Q\nonumber \end{aligned} \end{equation}
μˉt=Σˉt=Aμ^t−1+BAΣ^t−1AT+Q
P ( z t ∣ x 1 , ⋯ , x t ) P(z_t|x_1,\cdots,x_{t}) P(zt∣x1,⋯,xt)
求解这个问题,我们一样的采用前面的方法
先来看公式
假设给定
x
,
y
∼
N
(
(
μ
x
μ
y
)
,
(
Σ
x
x
Σ
x
y
Σ
y
x
Σ
y
y
)
)
(4)
x,y \sim N\begin{pmatrix} \begin{pmatrix} \mu_x \\ \mu_y \end{pmatrix}, \begin{pmatrix} \Sigma_{xx} &\Sigma_{xy} \\ \Sigma_{yx} & \Sigma_{yy} \end{pmatrix} \end{pmatrix}\tag{4}
x,y∼N((μxμy),(ΣxxΣyxΣxyΣyy))(4)
则
P
(
x
∣
y
)
∼
N
(
x
∣
μ
k
+
Σ
x
y
Σ
y
y
−
1
y
,
Σ
k
)
(5)
P(x|y) \sim N(x|\mu_k+\Sigma_{xy}\Sigma_{yy}^{-1}y,\Sigma_k)\tag{5}
P(x∣y)∼N(x∣μk+ΣxyΣyy−1y,Σk)(5)
其中
μ
k
=
μ
x
−
Σ
x
y
Σ
y
y
−
1
μ
y
\mu_k=\mu_x-\Sigma_{xy}\Sigma_{yy}^{-1}\mu_y
μk=μx−ΣxyΣyy−1μy,
Σ
k
=
Σ
x
x
−
Σ
x
y
Σ
y
y
−
1
Σ
y
x
\Sigma_k=\Sigma_{xx}-\Sigma_{xy}\Sigma_{yy}^{-1}\Sigma_{yx}
Σk=Σxx−ΣxyΣyy−1Σyx
啥意思呢?意思就是假设联合随机变量 x x x和 y y y服从公式4,则我们能够得出公式5。其实也就是如果能够得到联合概率分布的参数,那么我们就可以求出其条件概率的参数。推导过程参见线性动态系统中的概率求解-CSDN博客
那么,这个东西有什么用捏?我们仔细来看,我们可以不可以将 P ( z t , x t ∣ x 1 , ⋯ , x t − 1 ) P(z_t,x_{t}|x_1,\cdots,x_{t-1}) P(zt,xt∣x1,⋯,xt−1)当作是 P ( x , y ) P(x,y) P(x,y),那么 P ( x ∣ y ) P(x|y) P(x∣y)不就是我们要求的 P ( z t ∣ x 1 , ⋯ , x t ) P(z_t|x_1,\cdots,x_t) P(zt∣x1,⋯,xt)
没错啊,所以我们只需要求出 P ( z t , x t ∣ x 1 , ⋯ , x t − 1 ) P(z_t,x_{t}|x_1,\cdots,x_{t-1}) P(zt,xt∣x1,⋯,xt−1)即可。也就是要求出它里面对应的参数。如果
对于这个联合分布,我们当燃是叠罗汉式地将 P ( z t ∣ x 1 , ⋯ , x t − 1 ) P(z_t|x_1,\cdots,x_{t-1}) P(zt∣x1,⋯,xt−1), P ( x t ∣ x 1 , ⋯ , x t − 1 ) P(x_t|x_1,\cdots,x_{t-1}) P(xt∣x1,⋯,xt−1),而第一个我们已经在前面解出来过了。
接下来我们来看
P
(
x
t
∣
x
1
,
⋯
,
x
t
−
1
)
P(x_t|x_1,\cdots,x_{t-1})
P(xt∣x1,⋯,xt−1)
P
(
x
t
∣
x
1
,
⋯
,
x
t
−
1
)
=
∫
z
t
P
(
x
t
,
z
t
∣
x
1
,
⋯
,
x
t
−
1
)
d
z
t
=
∫
z
t
P
(
x
t
∣
x
1
,
⋯
,
x
t
−
1
,
z
t
)
P
(
z
t
∣
x
1
,
⋯
,
x
t
−
1
)
d
z
t
=
∫
z
t
P
(
x
t
∣
z
t
)
P
(
z
t
∣
x
1
,
⋯
,
x
t
−
1
)
d
z
t
\begin{equation} \begin{aligned} P(x_{t}|x_1,\cdots,x_{t-1})=&\int_{z_t}P(x_t,z_t|x_1,\cdots,x_{t-1})d{z_t} \\=&\int_{z_t}P(x_t|x_1,\cdots,x_{t-1},z_{t})P(z_t|x_1,\cdots,x_{t-1})dz_t \\=&\int_{z_t}P(x_t|z_t)P(z_t|x_1,\cdots,x_{t-1})dz_t \nonumber \end{aligned} \end{equation}
P(xt∣x1,⋯,xt−1)===∫ztP(xt,zt∣x1,⋯,xt−1)dzt∫ztP(xt∣x1,⋯,xt−1,zt)P(zt∣x1,⋯,xt−1)dzt∫ztP(xt∣zt)P(zt∣x1,⋯,xt−1)dzt
发现了没有,对于
P
(
z
t
∣
x
1
,
⋯
,
x
t
−
1
)
P(z_t|x_1,\cdots,x_{t-1})
P(zt∣x1,⋯,xt−1),就是我们前面求出来的,而对于
P
(
x
t
∣
z
t
)
P(x_t|z_t)
P(xt∣zt),我们前面是一样的有
x
t
=
C
z
t
+
D
+
v
x_t=Cz_t+D+v
xt=Czt+D+v。
所以,同前面求 P ( z t ∣ x 1 , ⋯ , x t − 1 ) P(z_t|x_1,\cdots,x_{t-1}) P(zt∣x1,⋯,xt−1)一样,我们将 P ( z r ∣ x 1 , ⋯ , x t − 1 ) P(z_r|x_1,\cdots,x_{t-1}) P(zr∣x1,⋯,xt−1)当作是 P ( x ) P(x) P(x),将 P ( x t ∣ x 1 , ⋯ , x t − 1 , z t ) P(x_t|x_1,\cdots,x_{t-1},z_{t}) P(xt∣x1,⋯,xt−1,zt)当作是 P ( y ∣ x ) P(y|x) P(y∣x),而我们所要求解的 P ( x t ∣ x 1 , ⋯ , x t − 1 ) P(x_t|x_1,\cdots,x_{t-1}) P(xt∣x1,⋯,xt−1)理所当然就是 P ( y ) P(y) P(y).
所以,接下来我们按部就班,我们设它的期望和协方差矩阵为
μ
x
,
Σ
x
\mu_x,\Sigma_x
μx,Σx
μ
x
=
C
μ
ˉ
t
+
D
Σ
x
=
C
Σ
ˉ
t
C
T
+
R
\begin{equation} \begin{aligned} \mu_x=&C\bar\mu_{t}+D \\\Sigma_x=&C\bar\Sigma_{t}C^T+R\nonumber \end{aligned} \end{equation}
μx=Σx=Cμˉt+DCΣˉtCT+R
好。现在我们就可以叠罗汉了
(
z
t
,
x
t
∣
x
1
,
⋯
,
x
t
−
1
)
∼
N
(
(
μ
ˉ
t
C
μ
ˉ
t
+
D
)
,
(
Σ
ˉ
t
Σ
x
y
Σ
y
x
C
Σ
ˉ
t
C
T
+
R
)
)
(z_t,x_{t}|x_1,\cdots,x_{t-1}) \sim N\begin{pmatrix} \begin{pmatrix} \bar\mu_t \\ C\bar\mu_{t}+D \end{pmatrix}, \begin{pmatrix} \bar\Sigma_{t} &\Sigma_{xy} \\ \Sigma_{yx} & C\bar\Sigma_{t}C^T+R\nonumber \end{pmatrix} \end{pmatrix}
(zt,xt∣x1,⋯,xt−1)∼N((μˉtCμˉt+D),(ΣˉtΣyxΣxyCΣˉtCT+R))
那么接下来就只剩下
Σ
x
y
\Sigma_{xy}
Σxy,至于这个东西怎么求解,我们在公式2中也给出了呃,在线性动态系统中的概率求解-CSDN博客也有推导了,最终
Σ
x
y
=
Σ
ˉ
t
C
T
\Sigma_{xy}=\bar\Sigma_{t}C^T
Σxy=ΣˉtCT,而
Σ
y
x
\Sigma_{yx}
Σyx也就是
C
Σ
ˉ
t
C\bar\Sigma_{t}
CΣˉt
所以,最终得到
(
z
t
,
x
t
∣
x
1
,
⋯
,
x
t
−
1
)
∼
N
(
(
μ
ˉ
t
C
μ
ˉ
t
+
D
)
,
(
Σ
ˉ
t
Σ
ˉ
t
C
T
C
Σ
ˉ
t
C
Σ
ˉ
t
C
T
+
R
)
)
(z_t,x_{t}|x_1,\cdots,x_{t-1}) \sim N\begin{pmatrix} \begin{pmatrix} \bar\mu_t \\ C\bar\mu_{t}+D \end{pmatrix}, \begin{pmatrix} \bar\Sigma_{t} &\bar\Sigma_{t}C^T \\ C\bar\Sigma_{t} & C\bar\Sigma_{t}C^T+R\nonumber \end{pmatrix} \end{pmatrix}
(zt,xt∣x1,⋯,xt−1)∼N((μˉtCμˉt+D),(ΣˉtCΣˉtΣˉtCTCΣˉtCT+R))
所以对于条件概率
P
(
z
t
∣
x
1
,
⋯
,
x
t
)
P(z_t|x_1,\cdots,x_{t})
P(zt∣x1,⋯,xt),我们只需要代公式即可求出,将公式5里面的值都代换,即可得到期望和协方差矩阵
我们先回顾一下公式5
P
(
x
∣
y
)
∼
N
(
x
∣
μ
k
+
Σ
x
y
Σ
y
y
−
1
y
,
Σ
k
)
P(x|y) \sim N(x|\mu_k+\Sigma_{xy}\Sigma_{yy}^{-1}y,\Sigma_k)
P(x∣y)∼N(x∣μk+ΣxyΣyy−1y,Σk)
其中
μ
k
=
μ
x
−
Σ
x
y
Σ
y
y
−
1
μ
y
\mu_k=\mu_x-\Sigma_{xy}\Sigma_{yy}^{-1}\mu_y
μk=μx−ΣxyΣyy−1μy,
Σ
k
=
Σ
x
x
−
Σ
x
y
Σ
y
y
−
1
Σ
y
x
\Sigma_k=\Sigma_{xx}-\Sigma_{xy}\Sigma_{yy}^{-1}\Sigma_{yx}
Σk=Σxx−ΣxyΣyy−1Σyx
我们定义
K
=
Σ
x
y
Σ
y
y
−
1
=
Σ
ˉ
t
C
T
(
C
Σ
ˉ
t
C
T
+
R
)
−
1
\begin{equation} \begin{aligned} K=&\Sigma_{xy}\Sigma_{yy}^{-1} \\=&\bar\Sigma_tC^T\left(C\bar\Sigma_{t}C^T+R\right)^{-1} \nonumber \end{aligned} \end{equation}
K==ΣxyΣyy−1ΣˉtCT(CΣˉtCT+R)−1
所以期望为
μ
^
t
=
μ
k
+
Σ
x
y
Σ
y
y
−
1
y
=
μ
ˉ
t
−
Σ
x
y
Σ
y
y
−
1
μ
y
+
Σ
x
y
Σ
y
y
−
1
y
=
μ
ˉ
t
−
K
(
C
μ
ˉ
t
+
D
)
+
K
x
t
=
μ
ˉ
t
+
K
(
x
t
−
C
μ
ˉ
t
−
D
)
\begin{equation} \begin{aligned} \hat\mu_{t}=&\mu_k+\Sigma_{xy}\Sigma_{yy}^{-1}y \\=&\bar\mu_t-\Sigma_{xy}\Sigma_{yy}^{-1}\mu_y+\Sigma_{xy}\Sigma_{yy}^{-1}y \\=&\bar \mu_t-K(C\bar\mu_t+D)+Kx_t \\=&\bar \mu_t+K(x_t-C\bar\mu_t-D)\nonumber \end{aligned} \end{equation}
μ^t====μk+ΣxyΣyy−1yμˉt−ΣxyΣyy−1μy+ΣxyΣyy−1yμˉt−K(Cμˉt+D)+Kxtμˉt+K(xt−Cμˉt−D)
协方差矩阵为
Σ
^
t
=
Σ
x
x
−
Σ
x
y
Σ
y
y
−
1
Σ
x
y
=
Σ
ˉ
t
−
K
C
Σ
ˉ
t
=
(
I
−
K
C
)
Σ
ˉ
t
\begin{equation} \begin{aligned} \hat\Sigma_t=&\Sigma_{xx}-\Sigma_{xy}\Sigma_{yy}^{-1}\Sigma_{xy} \\=&\bar\Sigma_{t}-KC\bar\Sigma_{t} \\=&(I-KC)\bar\Sigma_{t} \nonumber \end{aligned} \end{equation}
Σ^t===Σxx−ΣxyΣyy−1ΣxyΣˉt−KCΣˉt(I−KC)Σˉt
其中
I
I
I为单位矩阵
结论
至此,我们就推导出来了广为人知的卡尔曼黄金五式,即
①预测
μ
ˉ
t
=
A
μ
^
t
−
1
+
B
Σ
ˉ
t
=
A
Σ
^
t
−
1
A
T
+
Q
\begin{equation} \begin{aligned} \bar\mu_t=&A\hat\mu_{t-1}+B \\\bar\Sigma_t=&A\hat\Sigma_{t-1}A^T+Q\nonumber \end{aligned} \end{equation}
μˉt=Σˉt=Aμ^t−1+BAΣ^t−1AT+Q
②更新
K
=
Σ
ˉ
t
C
T
(
C
Σ
ˉ
t
C
T
+
R
)
−
1
μ
^
t
=
μ
ˉ
t
+
K
(
x
t
−
C
μ
ˉ
t
−
D
)
Σ
^
t
=
(
I
−
K
C
)
Σ
ˉ
t
K=\bar\Sigma_tC^T\left(C\bar\Sigma_{t}C^T+R\right)^{-1} \\\hat\mu_{t}=\bar \mu_t+K(x_t-C\bar\mu_t-D)\nonumber \\\hat\Sigma_t=(I-KC)\bar\Sigma_{t}
K=ΣˉtCT(CΣˉtCT+R)−1μ^t=μˉt+K(xt−Cμˉt−D)Σ^t=(I−KC)Σˉt
其中
K
K
K被称为卡尔曼增益
结束
到此,卡尔曼滤波的Filter问题已经全部推导完成。如有错误,还望指出。阿里嘎多。