前言
贝叶斯滤波(2)从数学角度上给出了一般系统下的贝叶斯滤波流程。本文将着重于贝叶斯滤波在移动机器人上的应用。
移动机器人建模
建模
假设移动机器人系统(位置,速度)具有马尔可夫性,满足状态方程和观测方程,其有噪状态观测是 Z = { z 1 , z 2 , … , z n } Z=\{ z_1,\ z_2, \ \dots, z_n \} Z={z1, z2, …,zn},上一时刻的状态最优估计是 x n − 1 x_{n-1} xn−1,本时刻的输入是 u n u_n un,观测是 z n z_n zn。
已知
根据状态方程,可从上一时刻的状态最优估计 x n − 1 x_{n-1} xn−1与本时刻的输入 u n u_n un,获得本时刻的状态预测 p ( x n ∣ x n − 1 , u n ) p(x_n|x_{n-1},u_n) p(xn∣xn−1,un)
根据观测方程,获得本时刻的状态观测 p ( z n ∣ x n ) p(z_n|x_n) p(zn∣xn)
目标
估计本时刻系统的真实状态 x n x_n xn,即 p ( x n ∣ x n − 1 , u n , z n ) p(x_n|x_{n-1},u_n,z_n) p(xn∣xn−1,un,zn)
移动机器人状态估计
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
,
u
n
)
p
(
z
n
)
=
p
(
z
n
∣
x
n
)
p
(
x
n
∣
x
n
−
1
,
u
n
)
∫
p
(
z
n
∣
x
n
,
x
n
−
1
,
u
n
)
p
(
x
n
∣
x
n
−
1
,
u
n
)
d
x
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},u_n)} {p(z_n)} \\ &= \frac {p(z_n|x_n)p(x_n|x_{n-1},u_n)} {\int p(z_n|x_n,x_{n-1},u_n)p(x_n|x_{n-1},u_n)dx_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,un)=∫p(zn∣xn,xn−1,un)p(xn∣xn−1,un)dxnp(zn∣xn)p(xn∣xn−1,un)
马尔科夫性:
p
(
z
n
∣
x
n
,
x
n
−
1
,
u
n
)
=
p
(
z
n
∣
x
n
)
p(z_n|x_n,x_{n-1},u_n) =p(z_n|x_n)
p(zn∣xn,xn−1,un)=p(zn∣xn)
全概率公式:
p
(
z
n
)
=
∫
p
(
z
n
∣
x
n
,
x
n
−
1
,
u
n
)
p
(
x
n
∣
x
n
−
1
,
u
n
)
d
x
n
∈
R
p(z_n) = \int p(z_n|x_n,x_{n-1},u_n)p(x_n|x_{n-1},u_n)dx_n \in R
p(zn)=∫p(zn∣xn,xn−1,un)p(xn∣xn−1,un)dxn∈R
后记
本篇与上篇贝叶斯滤波都是在讲贝叶斯滤波的流程。
不同的是,上篇侧重于数学的角度来看,状态预测的过程是由 z 1 : n − 1 z_{1:n-1} z1:n−1构建的。
本篇侧重于实际机器人应用角度,状态预测的过程是由上一时刻的最优估计 x n − 1 x_{n-1} xn−1与观测方程构建的,从而和卡尔曼滤波建立联系。