控制算法学习 一、卡尔曼滤波（2）从贝叶斯的角度
- 前言
- 系统建模
- 贝叶斯滤波
- 先验
- 似然
- 后验
前言
控制算法学习一(1)是以小车为例，推演卡尔曼滤波算法的流程。
举例易于理解算法，但如果要真正掌握算法，必须从算法的本质入手进行推导。
卡尔曼滤波的本质，就是贝叶斯滤波在高斯过程上处理的特例。
系统建模
仍然以小车模型为例：
x
k
=
A
x
k
−
1
+
B
u
k
−
1
+
w
k
−
1
,
w
k
∼
[
0
,
Q
]
z
k
=
H
x
k
+
v
k
,
v
k
∼
[
0
,
R
]
\begin{aligned} & x_k = Ax_{k-1} + Bu_{k-1} + w_{k-1}, w_k\sim [0, Q] \\ & z_k = Hx_k+v_{k} ,v_k\sim[0, R] \end{aligned}
xk=Axk−1+Buk−1+wk−1,wk∼[0,Q]zk=Hxk+vk,vk∼[0,R]
A
,
B
,
H
A,B,H
A,B,H分别表示状态转移矩阵，控制（输入）转换矩阵，观测转换矩阵。
w k , v k w_k,v_k wk,vk都是高斯白噪声，表示了系统误差（过程噪声）和测量误差。
x k , z k x_k,z_k xk,zk是向量，表明k时刻的系统状态。
小车系统是具有马尔可夫性的，并假设它的位置和速度服从高斯分布，其有噪状态观测 Z = { z 1 , z 2 , … , z n } Z=\{ z_1,\ z_2, \ \dots, z_n \} Z={z1, z2, …,zn}，本时刻的观测是 z n z_n zn，卡尔曼滤波的目标是获得本时刻系统真实状态的最优估计 x n x_n xn。
重点：系统具有马尔科夫性，并且系统状态服从高斯分布
贝叶斯滤波
由贝叶斯滤波（3）得到：
p
(
x
n
∣
x
n
−
1
,
u
n
,
z
n
)
=
p
(
z
n
∣
x
n
,
x
n
−
1
,
u
n
)
p
(
x
n
∣
x
n
−
1
,
u
n
)
p
(
z
n
∣
x
n
−
1
,
u
n
)
=
p
(
z
n
∣
x
n
)
p
(
x
n
∣
x
n
−
1
,
u
n
)
p
(
z
n
∣
x
n
−
1
,
u
n
)
=
p
(
z
n
∣
x
n
)
p
(
x
n
∣
x
n
−
1
)
p
(
z
n
)
\begin{aligned} p(x_n|x_{n-1},u_n,z_n) & = \frac {p(z_n|x_n,x_{n-1},u_n)p(x_n|x_{n-1},u_n)} {p(z_n|x_{n-1},u_n)} \\ &= \frac {p(z_n|x_n)p(x_n|x_{n-1},u_n)} {p(z_n|x_{n-1},u_n)} \\ &= \frac {p(z_n|x_n)p(x_n|x_{n-1})} {p(z_n)} \\ \end{aligned}
p(xn∣xn−1,un,zn)=p(zn∣xn−1,un)p(zn∣xn,xn−1,un)p(xn∣xn−1,un)=p(zn∣xn−1,un)p(zn∣xn)p(xn∣xn−1,un)=p(zn)p(zn∣xn)p(xn∣xn−1)
控制输入量
u
n
u_n
un并不本质，推导省略控制输入
先验
上一时刻的状态最优估计是当前状态的先验，根据高斯分布的性质计算
p
(
x
n
∣
x
n
−
1
)
p(x_n|x_{n-1})
p(xn∣xn−1)：
x
n
=
A
x
n
−
1
+
w
n
−
1
,
w
n
∼
[
0
,
Q
]
x
~
n
∼
N
[
A
x
n
−
1
,
A
P
n
−
1
A
T
+
Q
]
x_n=Ax_{n-1} + w_{n-1}, w_n\sim [0,Q] \\ \tilde x_{n} \sim N[Ax_{n-1}, AP_{n-1}A^T+Q]
xn=Axn−1+wn−1,wn∼[0,Q]x~n∼N[Axn−1,APn−1AT+Q]
这里的
x
n
−
1
x_{n-1}
xn−1即上一时刻系统状态的最优估计。
x
~
n
\tilde x_{n}
x~n即贝叶斯思想里的先验。
似然
观测得到的状态是真实状态的似然
p
(
z
n
∣
x
n
)
p(z_n|x_n)
p(zn∣xn)：
z
n
=
H
x
n
+
v
n
,
v
n
∼
N
[
0
,
R
]
z_n = Hx_n+v_{n} ,v_n\sim N[0, R]
zn=Hxn+vn,vn∼N[0,R]
后验
后验 ∝ \propto ∝先验 × \times ×似然
p
(
x
n
∣
x
n
−
1
,
z
n
)
=
p
(
z
n
∣
x
n
)
p
(
x
n
∣
x
n
−
1
)
p
(
z
n
)
p(x_n|x_{n-1},z_n) = \frac {p(z_n|x_n)p(x_n|x_{n-1})} {p(z_n)} \\
p(xn∣xn−1,zn)=p(zn)p(zn∣xn)p(xn∣xn−1)
先验、似然均已知，通过卡尔曼的预测和更新过程的公式就可以把
p
(
x
n
∣
x
n
−
1
,
z
n
)
p(x_n|x_{n-1},z_n)
p(xn∣xn−1,zn)的具体表达式求出。
p
(
x
n
∣
x
n
−
1
,
z
n
)
p(x_n|x_{n-1},z_n)
p(xn∣xn−1,zn)的期望和协方差就作为当前时刻的状态最优估计。
