本文详细介绍了贝叶斯滤波的基本概念，包括运动和观测模型，以及非线性运动和观测函数。通过数学推导，展示了如何利用贝叶斯公式和一阶马尔可夫性来更新和预测系统状态的概率分布，从而实现对未知状态的估计。该过程涉及到后验置信度、先验置信度和利用观测模型进行更新等关键步骤。
贝叶斯滤波器
定义以下运动和观测模型：
运动方程：xk=f(xk−1,vk,wk),k=1,⋯ ,K观测方程：yk=g(xk,nk),k=0,⋯ ,K \begin{aligned}&\text{运动方程：}\boldsymbol{x}_{k}=f\left(\boldsymbol{x}_{k-1}, \boldsymbol{v}_{k},\boldsymbol{w}_{k}\right), \quad k=1, \cdots, K \\&\text{观测方程：}\boldsymbol{y}_{k}=g\left(\boldsymbol{x}_{k}, \boldsymbol{n}_{k}\right), \quad k=0, \cdots, K\end{aligned} 运动方程：xk=f(xk−1,vk,wk),k=1,⋯,K观测方程：yk=g(xk,nk),k=0,⋯,K
变量和符号定义如下：
- xk∈RN\boldsymbol{x}_{k} \in \mathbb{R}^{N}xk∈RN 表示系统状态
- x0∈RN\boldsymbol{x}_{0} \in \mathbb{R}^{N}x0∈RN 表示初始状态
- vk∈RN\boldsymbol{v}_{k} \in \mathbb{R}^{N}vk∈RN 表示输入
- wk∈RN\boldsymbol{w}_{k} \in \mathbb{R}^{N}wk∈RN 表示过程噪声
- yk∈RM\boldsymbol{y}_{k} \in \mathbb{R}^{M}yk∈RM 表示测量
- nk∈RM\boldsymbol{n}_{k} \in \mathbb{R}^{M}nk∈RM 表示测量噪声
- kkk 为时间下标，最大值为 KKK
- 函数 f(⋅)f(\cdot)f(⋅) 为非线性的运动模型
- 函数 g(⋅)g(\cdot)g(⋅) 为非线性的观测模型
贝叶斯滤波仅使用过去以及当前的测量，通过构造一个完整的概率密度函数PDF，来计算当前状态 xk\boldsymbol{x}_{k}xk 的置信度：
P(xk∣xˇ0,v1:k,y0:k)(1) P\left(\boldsymbol{x}_{k} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k}\right) \tag{1} P(xk∣xˇ0,v1:k,y0:k)(1)
其中， xˇ0\check{\boldsymbol{x}}_{0}xˇ0 表示 0 时刻系统状态的先验。
贝叶斯滤波器可以用下面公式表示：
p(xk∣xˇ0,v1:k,y0:k)⏟后验置信度 =ηp(yk∣xk)⏟利用 g(⋅) 进行更新 ∫p(xk∣xk−1,vk)⏟利用 f(⋅) 进行预测 p(xk−1∣xˇ0,v1:k−1,y0:k−1)⏟先验置信度 dxk−1(2) \begin{aligned}& \underbrace{p\left(\boldsymbol{x}_{k} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k}\right)}_{\text {后验置信度 }} \\=& \eta \underbrace{p\left(\boldsymbol{y}_{k} \mid \boldsymbol{x}_{k}\right)}_{\text {利用 } \boldsymbol{g}(\cdot) \text { 进行更新 }} \int \underbrace{p\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{v}_{k}\right)}_{\text {利用 } \boldsymbol{f}(\cdot) \text { 进行预测 }} \underbrace{p\left(\boldsymbol{x}_{k-1} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k-1}, \boldsymbol{y}_{0: k-1}\right)}_{\text {先验置信度 }} \mathrm{d} \boldsymbol{x}_{k-1}\end{aligned}\tag{2} =后验置信度 p(xk∣xˇ0,v1:k,y0:k)η利用 g(⋅) 进行更新 p(yk∣xk)∫利用 f(⋅) 进行预测 p(xk∣xk−1,vk)先验置信度 p(xk−1∣xˇ0,v1:k−1,y0:k−1)dxk−1(2)
推导过程
对（1）式进行贝叶斯展开，有：
p(xk∣xˇ0,v1:k,y0:k)=p(xk,xˇ0,v1:k,y0:k)p(xˇ0,v1:k,y0:k)=p(yk∣xk,xˇ0,v1:k,y0:k−1)p(xk∣xˇ0,v1:k,y0:k−1)p(xˇ0,v1:k,y0:k−1)p(yk∣xˇ0,v1:k,y0:k−1)p(xˇ0,v1:k,y0:k−1)=p(yk∣xk,xˇ0,v1:k,y0:k−1)p(xk∣xˇ0,v1:k,y0:k−1)p(yk∣xˇ0,v1:k,y0:k−1)(3) \begin{aligned}& p\left(\boldsymbol{x}_{k} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k}\right) \\\\&= \frac{p\left(\boldsymbol{x}_{k}, \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k}\right)}{p\left(\check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k}\right) } \\\\&= \frac{p\left(\boldsymbol{y}_{ k} \mid \boldsymbol{x}_{k}, \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right) p\left(\boldsymbol{x}_{k} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right)p\left( \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right)}{p\left(\boldsymbol{y}_{ k} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right) p\left( \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right)} \\\\&=\frac{p\left(\boldsymbol{y}_{ k} \mid \boldsymbol{x}_{k}, \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right) p\left(\boldsymbol{x}_{k} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right)}{p\left(\boldsymbol{y}_{ k} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right) }\end{aligned}\tag{3} p(xk∣xˇ0,v1:k,y0:k)=p(xˇ0,v1:k,y0:k)p(xk,xˇ0,v1:k,y0:k)=p(yk∣xˇ0,v1:k,y0:k−1)p(xˇ0,v1:k,y0:k−1)p(yk∣xk,xˇ0,v1:k,y0:k−1)p(xk∣xˇ0,v1:k,y0:k−1)p(xˇ0,v1:k,y0:k−1)=p(yk∣xˇ0,v1:k,y0:k−1)p(yk∣xk,xˇ0,v1:k,y0:k−1)p(xk∣xˇ0,v1:k,y0:k−1)(3)
由于 yk\boldsymbol{y}_kyk 的状态只与 xk\boldsymbol{x}_kxk 相关，所以上式可化简为：
p(xk∣xˇ0,v1:k,y0:k)=p(yk∣xk)p(xk∣xˇ0,v1:k,y0:k−1)p(yk)(4) \begin{aligned}& p\left(\boldsymbol{x}_{k} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k}\right) \\\\&= \frac{p\left(\boldsymbol{y}_{ k} \mid \boldsymbol{x}_{k}\right) p\left(\boldsymbol{x}_{k} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right)}{p\left(\boldsymbol{y}_k\right) }\end{aligned}\tag{4} p(xk∣xˇ0,v1:k,y0:k)=p(yk)p(yk∣xk)p(xk∣xˇ0,v1:k,y0:k−1)(4)
令 p−1(yk)=ηp^{-1}\left(\boldsymbol{y}_k\right) = \etap−1(yk)=η，则有：
p(xk∣xˇ0,v1:k,y0:k)=ηp(yk∣xk)p(xk∣xˇ0,v1:k,y0:k−1)(5) \begin{aligned}& p\left(\boldsymbol{x}_{k} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k}\right) = \eta p\left(\boldsymbol{y}_{ k} \mid \boldsymbol{x}_{k}\right) p\left(\boldsymbol{x}_{k} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right) \end{aligned}\tag{5} p(xk∣xˇ0,v1:k,y0:k)=ηp(yk∣xk)p(xk∣xˇ0,v1:k,y0:k−1)(5)
由全概率公式和贝叶斯公式有：
p(xk∣xˇ0,v1:k,y0:k−1)=∫p(xk,xk−1∣xˇ0,v1:k,y0:k−1)dxk−1=∫p(xk∣xk−1,xˇ0,v1:k,y0:k−1)p(xk−1∣xˇ0,v1:k,y0:k−1)dxk−1(6) \begin{aligned}& p\left(\boldsymbol{x}_{k} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right)\\\\=&\int p\left(\boldsymbol{x}_{k}, \boldsymbol{x}_{k-1} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right) \mathrm{d} \boldsymbol{x}_{k-1} \\\\=& \int p\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{k-1}, \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right) p\left(\boldsymbol{x}_{k-1} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right) \mathrm{d} \boldsymbol{x}_{k-1}\end{aligned}\tag{6} ==p(xk∣xˇ0,v1:k,y0:k−1)∫p(xk,xk−1∣xˇ0,v1:k,y0:k−1)dxk−1∫p(xk∣xk−1,xˇ0,v1:k,y0:k−1)p(xk−1∣xˇ0,v1:k,y0:k−1)dxk−1(6)
然后，由一阶马尔可夫性假设（ k 时刻的状态只和 k - 1 时刻状态有关）有：
p(xk∣xk−1,xˇ0,v1:k,y0:k−1)=p(xk∣xk−1,vk)(7) p\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{k-1}, \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right)=p\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{k-1},\boldsymbol{v}_{k}\right)\tag{7} p(xk∣xk−1,xˇ0,v1:k,y0:k−1)=p(xk∣xk−1,vk)(7)
并且 k - 1 时刻的状态与 k 时刻的输入无关，即：
p(xk−1∣xˇ0,v1:k,y0:k−1)=p(xk−1∣xˇ0,v1:k−1,y0:k−1)(8) p\left(\boldsymbol{x}_{k-1} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k-1}\right) = p\left(\boldsymbol{x}_{k-1} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k-1}, \boldsymbol{y}_{0: k-1}\right) \tag{8} p(xk−1∣xˇ0,v1:k,y0:k−1)=p(xk−1∣xˇ0,v1:k−1,y0:k−1)(8)
结合 (5)、(6)、(7)、(8) 式，即可得到：
p(xk∣xˇ0,v1:k,y0:k)=ηp(yk∣xk)∫p(xk∣xk−1,vk)p(xk−1∣xˇ0,v1:k−1,y0:k−1)dxk−1(9) \begin{aligned}& p\left(\boldsymbol{x}_{k} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k}, \boldsymbol{y}_{0: k}\right) \\=& \eta p\left(\boldsymbol{y}_{ k} \mid \boldsymbol{x}_{k}\right) \int p\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{k-1},\boldsymbol{v}_{k}\right)p\left(\boldsymbol{x}_{k-1} \mid \check{\boldsymbol{x}}_{0}, \boldsymbol{v}_{1: k-1}, \boldsymbol{y}_{0: k-1}\right) \mathrm{d} \boldsymbol{x}_{k-1}\end{aligned}\tag{9} =p(xk∣xˇ0,v1:k,y0:k)ηp(yk∣xk)∫p(xk∣xk−1,vk)p(xk−1∣xˇ0,v1:k−1,y0:k−1)dxk−1(9)
即证。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
