贝叶斯滤波推导与简析
一、数学基础充能
1、贝叶斯公式
p ( A ∣ B ) = p ( B ∣ A ) p ( A ) P ( B ) p\left( {A|B} \right) = \frac{{p\left( {B|A} \right)p\left( A \right)}}{{P\left( B \right)}}p(A∣B)=P(B)p(B∣A)p(A)
2、全概率公式
p ( B ) = ∑ i = 1 n p ( B ∣ A i ) p ( A i ) p\left( B \right) = \sum\limits_{i = 1}^n {p\left( {B|{A_i}} \right)p\left( {{A_i}} \right)}p(B)=i=1∑np(B∣Ai)p(Ai)
二、推导过程
假设非线性系统具有加性噪声，描述为：
x k + 1 = f k ( x k ) + w k z k = h k ( x k ) + v k \begin{array}{l} {x_{k + 1}} = {f_k}\left( {{x_k}} \right) + {w_k}\\ \\ {z_k} = {h_k}\left( {{x_k}} \right) + {v_k} \end{array}xk+1=fk(xk)+wkzk=hk(xk)+vk
其中，k ∈ N k \in Nk∈N是时间指标，x k ∈ R n {x_k} \in {R^n}xk∈Rn是k kk时刻系统的状态向量，f k : R n → R n {f_k}:{R^n} \to {R^n}fk:Rn→Rn是状态转移映射，w k {w_k}wk是n nn维过程噪声，z k ∈ R m {z_k} \in {R^m}zk∈Rm是k kk时刻对系统状态的量测向量，h k : R n → R m {h_k}:{R^n} \to {R^m}hk:Rn→Rm是量测映射，v k {v_k}vk是m mm维量测噪声。
假定：
(1)、初始状态的概率密度函数已知，表示为
p ( x 0 ) p\left( {{x_0}} \right)p(x0)
(2)、过程噪声和量测噪声都是独立过程，且二者相互独立，它们与初始状态也相互独立。它们的概率密度函数也已知，分别表示为
ρ ( w k ) , υ ( v k ) \rho \left( {{w_k}} \right),\upsilon \left( {{v_k}} \right)ρ(wk),υ(vk)
(3)、所有的概率密度函数都可以计算得到。
所谓贝叶斯滤波，就是在每个时刻k kk，利用所获得的实时量测信息Z k {Z^k}Zk求得状态x k x_{k}xk的后验概率密度函数
p ( x k ∣ Z k ) p\left( {{x_k}|{Z^k}} \right)p(xk∣Zk)
从而得到k kk时刻的状态估计及其估计误差协方差阵
x ^ k ∣ k = ∫ R n x k p ( x k ∣ Z k ) d x k P k ∣ k = ∫ R n ( x k − x ^ k ∣ k ) ( x k − x ^ k ∣ k ) T p ( x k ∣ Z k ) d x k \begin{array}{l} {{\hat x}_{k|k}} = \int_{{R^n}} {{x_k}p({x_k}|{Z^k})d{x_k}} \\ \\ {P_{k|k}} = \int_{{R^n}} {({x_k} - {{\hat x}_{k|k}})} {({x_k} - {{\hat x}_{k|k}})^T}p({x_k}|{Z^k})d{x_k} \end{array}x^k∣k=∫Rnxkp(xk∣Zk)dxkPk∣k=∫Rn(xk−x^k∣k)(xk−x^k∣k)Tp(xk∣Zk)dxk
其中，Z k = { z 1 , z 2 , ⋯ , z k } {Z^k} = \left\{ {{z_1},{z_2}, \cdots ,{z_k}} \right\}Zk={z1,z2,⋯,zk}表示初始时刻到k kk时刻所有的量测信息。
贝叶斯滤波步骤如下：
(1)、假定k − 1 k-1k−1时刻已经获得了后验概率密度p ( x k − 1 ∣ Z k − 1 ) p({x_{k - 1}}|{Z^{k - 1}})p(xk−1∣Zk−1)，那么状态一步预测的概率密度函数为
p ( x k ∣ Z k − 1 ) = ∫ R n p ( x k , x k − 1 ∣ Z k − 1 ) d x k − 1 = ∫ R n p ( x k ∣ x k − 1 , Z k − 1 ) p ( x k − 1 ∣ Z k − 1 ) d x k − 1 = ∫ R n p ( x k ∣ x k − 1 ) p ( x k − 1 ∣ Z k − 1 ) d x k − 1 \begin{array}{l} p({x_k}|{Z^{k - 1}}) = \int_{{R^n}} {p({x_k},{x_{k - 1}}|{Z^{k - 1}})d{x_{k - 1}}} \\ \\ = \int_{{R^n}} {p({x_k}|{x_{k - 1}},{Z^{k - 1}})p({x_{k - 1}}|{Z^{k - 1}})d{x_{k - 1}}} \\ \\ = \int_{{R^n}} {p({x_k}|{x_{k - 1}})p({x_{k - 1}}|{Z^{k - 1}})d{x_{k - 1}}} \end{array}p(xk∣Zk−1)=∫Rnp(xk,xk−1∣Zk−1)dxk−1=∫Rnp(xk∣xk−1,Zk−1)p(xk−1∣Zk−1)dxk−1=∫Rnp(xk∣xk−1)p(xk−1∣Zk−1)dxk−1
其中
p ( x k ∣ x k − 1 ) = ρ ( x k − f k − 1 ( x k − 1 ) ) p({x_k}|{x_{k - 1}}) = \rho ({x_k} - {f_{k - 1}}({x_{k - 1}}))p(xk∣xk−1)=ρ(xk−fk−1(xk−1))
(2)、在k kk时刻，已经获得新的量测数据z k z_{k}zk，利用贝叶斯公式计算得到后验概率密度函数
p ( x k ∣ Z k ) = p ( x k ∣ Z k − 1 , z k ) = p ( x k , z k ∣ Z k − 1 ) p ( z k ∣ Z k − 1 ) = p ( z k ∣ x k , Z k − 1 ) p ( x k ∣ Z k − 1 ) p ( z k ∣ Z k − 1 ) = p ( z k ∣ x k ) p ( x k ∣ Z k − 1 ) p ( z k ∣ Z k − 1 ) = υ ( z k − h k ( x k ) ) p ( x k ∣ Z k − 1 ) p ( z k ∣ Z k − 1 ) \begin{array}{l} p({x_k}|{Z^k}) = p({x_k}|{Z^{k - 1}},{z_k}) \\ \\ = \frac{{p({x_k},{z_k}|{Z^{k - 1}})}}{{p({z_k}|{Z^{k - 1}})}}\\ \\ = \frac{{p({z_k}|{x_k},{Z^{k - 1}})p({x_k}|{Z^{k - 1}})}}{{p({z_k}|{Z^{k - 1}})}}\\ \\ = \frac{{p({z_k}|{x_k})p({x_k}|{Z^{k - 1}})}}{{p({z_k}|{Z^{k - 1}})}}\\ \\ = \frac{{\upsilon ({z_k} - {h_k}({x_k}))p({x_k}|{Z^{k - 1}})}}{{p({z_k}|{Z^{k - 1}})}} \end{array}p(xk∣Zk)=p(xk∣Zk−1,zk)=p(zk∣Zk−1)p(xk,zk∣Zk−1)=p(zk∣Zk−1)p(zk∣xk,Zk−1)p(xk∣Zk−1)=p(zk∣Zk−1)p(zk∣xk)p(xk∣Zk−1)=p(zk∣Zk−1)υ(zk−hk(xk))p(xk∣Zk−1)
其中，量测一步预测概率密度p ( x k ∣ Z k − 1 ) p({x_k}|{Z^{k - 1}})p(xk∣Zk−1)由下式计算：
p ( z k ∣ Z k − 1 ) = ∫ R n p ( z k , x k ∣ Z k − 1 ) d x k = ∫ R n p ( z k ∣ x k , Z k − 1 ) p ( x k ∣ Z k − 1 ) d x k = ∫ R n p ( z k ∣ x k ) p ( x k ∣ Z k − 1 ) d x k \begin{array}{l} p({z_k}|{Z^{k - 1}}) = \int_{{R^n}} {p({z_k},{x_k}|{Z^{k - 1}})d{x_k}} \\ \\ = \int_{{R^n}} {p({z_k}|{x_k},{Z^{k - 1}})p({x_k}|{Z^{k - 1}})d{x_k}} \\ \\ = \int_{{R^n}} {p({z_k}|{x_k})p({x_k}|{Z^{k - 1}})d{x_k}} \end{array}p(zk∣Zk−1)=∫Rnp(zk,xk∣Zk−1)dxk=∫Rnp(zk∣xk,Zk−1)p(xk∣Zk−1)dxk=∫Rnp(zk∣xk)p(xk∣Zk−1)dxk
其中
p ( z k ∣ x k ) = υ ( z k − h k ( x k ) ) p({z_k}|{x_k}) = \upsilon ({z_k} - {h_k}({x_k}))p(zk∣xk)=υ(zk−hk(xk))
这样就完成了贝叶斯滤波的计算，但是过程中的高维积分往往没有解析解，因此贝叶斯滤波只是一个框架而不能直接应用。
三、简要注解
1、k kk时刻的过程噪声作用在k kk时刻的状态，影响到k + 1 k+1k+1时刻的状态，因此w ww的下标为k kk，状态方程写为：
x k + 1 = f k ( x k ) + w k {x_{k + 1}} = {f_k}\left( {{x_k}} \right) + {w_k}xk+1=fk(xk)+wk
2、k kk时刻的量测噪声作用在k kk时刻的量测上，直接影响k kk时刻的量测，因此v vv的下标为k kk，量测方程写为：
z k = h k ( x k ) + v k {z_k} = {h_k}\left( {{x_k}} \right) + {v_k}zk=hk(xk)+vk
3、k kk时刻的状态估计x ^ k ∣ k {\hat x_{k|k}}x^k∣k实际上就是k kk时刻的状态x k x_{k}xk在后验密度p ( x k ∣ Z k ) p\left( {{x_k}|{Z^k}} \right)p(xk∣Zk)下的数学期望。
4、贝叶斯滤波预测步：第一个等号类似于全概率公式；第二个等号运用了贝叶斯公式的变形；第三个等号根据现实情况，k kk时刻的状态与k − 1 k-1k−1时刻及以前的量测无关。p ( x k ∣ x k − 1 ) p({x_k}|{x_{k - 1}})p(xk∣xk−1)称为状态转移密度，由状态方程可以推得其计算方式为
p ( x k ∣ x k − 1 ) = ρ ( x k − f k − 1 ( x k − 1 ) ) p({x_k}|{x_{k - 1}}) = \rho ({x_k} - {f_{k - 1}}({x_{k - 1}}))p(xk∣xk−1)=ρ(xk−fk−1(xk−1))
这是为什么呢？首先，x k − 1 x_{k-1}xk−1现在作为条件，是已知的，因此f k − 1 ( x k − 1 ) {f_{k - 1}}\left( {{x_{k - 1}}} \right)fk−1(xk−1)也就可以视为常量，将其移到状态方程等号左边，即
x k − f k − 1 ( x k − 1 ) = w k {x_k} - {f_{k - 1}}\left( {{x_{k - 1}}} \right) = {w_k}xk−fk−1(xk−1)=wk
这个时候等号左右两边的随机向量是同分布的，因此将等号左边的内容代入到过程噪声的概率密度函数中，便可得到状态转移密度的计算式。
5、贝叶斯滤波更新步：也存在类似的问题，处理方法同上。