基于B站老王的贝叶斯滤波、卡尔曼滤波、粒子滤波 Bilibili
第三部分:随机过程的贝叶斯滤波BF
-
随机过程包含一系列随机变量 X 0 , X 1 , Y 1 , X 2 , Y 2 . . . X k , Y k X_0,X_1,Y_1,X_2,Y_2...X_k,Y_k X0,X1,Y1,X2,Y2...Xk,Yk,有一个初值 x 0 x_0 x0,有k个观测值 y 1 , y 2 , . . . , y k y_1,y_2,...,y_k y1,y2,...,yk
怎么办:
- 所有的 X 0 , . . . , X k X_0,...,X_k X0,...,Xk的先验概率都靠猜:过于依赖观测,放弃了预测信息。放弃了前后状态间的关系如 x k = x k − 1 + Q k x_k=x_{k-1}+Q_k xk=xk−1+Qk 。
- 只有 X 0 X_0 X0的概率是猜的, X 1 , . . . , X k X_1,...,X_k X1,...,Xk的先验概率靠递推(预测) √ \surd √
怎么做:
- 状态方程+观测方程(建模)
状态方程:描述 X k X_k Xk和 X k − 1 X_{k-1} Xk−1是什么关系 X k = f ( X k − 1 ) + Q k X_k=f(X_{k-1})+Q_k Xk=f(Xk−1)+Qk, Q k Q_k Qk:预测噪声
如 X k = 1 2 g t 2 + Q → 一 阶 T y l o r 展 开 X k = X k − 1 + X ˙ k − 1 ( t k − t k − 1 ) + Q = X k − 1 + g t ( t k − t k − 1 ) + Q 如X_k=\frac{1}{2}gt^2+Q \mathop{\to} \limits ^{一阶Tylor展开} X_k=X_{k-1}+\dot X_{k-1}(t_k-t_{k-1})+Q=X_{k-1}+gt(t_k-t_{k-1})+Q 如Xk=21gt2+Q→一阶Tylor展开Xk=Xk−1+X˙k−1(tk−tk−1)+Q=Xk−1+gt(tk−tk−1)+Q
状态方程可以建立的非常粗糙,只需要将方差Q调大,如 X k = X k − 1 + Q , Q ∼ N ( 0 , 1000 ) X_k=X_{k-1}+Q,Q\sim N(0,1000) Xk=Xk−1+Q,Q∼N(0,1000)。观测方程:反映了状态(温度、速度、角度)是如何引起传感器的读数, Y k = h ( X k ) + R k Y_k=h(X_k)+R_k Yk=h(Xk)+Rk, R k R_k Rk:观测噪声
-
如状态:温度,观测:温度, Y k = X k + R k Y_k=X_k+R_k Yk=Xk+Rk,温度+传感器误差。
-
状态:位移,观测:角度, Y k = a r c s i n X k + R k Y_k=arcsinX_k+R_k Yk=arcsinXk+Rk
{ x k = f ( x k − 1 ) + Q k 随 机 过 程 预 测 z k , j = h ( x k ) + R k 观 测 \left\{\begin{array}{l} \begin{aligned} x_k &= f(x_{k-1})+Q_k 随机过程预测\\ z_{k,j} &= h(x_k)+R_{k}观测 \end{aligned} \end{array}\right. {xkzk,j=f(xk−1)+Qk随机过程预测=h(xk)+Rk观测
怎么递推:
- 若无观测,设 X K = 2 X k − 1 X_K=2X_{k-1} XK=2Xk−1,首先设 X 0 ∼ N ( 0 , 1 ) X_0\sim N(0,1) X0∼N(0,1),那么 X 1 = 2 X 0 ∼ N ( 0 , 2 2 ) , X 2 = 2 X 1 ∼ N ( 0 , 2 4 ) X_1=2X_0\sim N(0,2^2),X_2=2X_1\sim N(0,2^4) X1=2X0∼N(0,22),X2=2X1∼N(0,24),方差越来越大,上一步的先验概率作为下一步的先验概率。
- 正确地递推:设
先
验
X
0
∼
N
(
0
,
1
)
→
预
测
步
先
验
X
1
−
∼
N
(
0
,
2
2
)
→
用
观
测
y
1
=
0
和
贝
叶
斯
公
式
更
新
步
/
后
验
步
后
验
/
先
验
X
1
+
∼
N
(
0
,
0.8
)
→
再
预
测
先
验
X
2
−
∼
N
(
0
,
3.2
)
→
观
测
y
2
=
0.1
再
更
新
后
验
X
2
+
.
.
.
先验X_0\sim N(0,1)\mathop{\to}\limits^{预测步}先验X_1^-\sim N(0,2^2)\mathop{\to}\limits^{用观测y_1=0和贝叶斯公式 更新步/后验步}后验/先验X_1^+\sim N(0,0.8)\mathop{\to}\limits^{再预测} 先验X_2^-\sim N(0,3.2)\mathop{\to}\limits^{观测y_2=0.1再更新}后验X_2^+...
先验X0∼N(0,1)→预测步先验X1−∼N(0,22)→用观测y1=0和贝叶斯公式更新步/后验步后验/先验X1+∼N(0,0.8)→再预测先验X2−∼N(0,3.2)→观测y2=0.1再更新后验X2+...,上一步的后验概率作为下一步的先验概率。
- 预测步:上一时刻的后验 → 状 态 方 程 \mathop{\to}\limits^{状态方程} →状态方程这一时刻的先验
- 更新步:这一时刻的先验 → 观 测 方 程 \mathop{\to}\limits^{观测方程} →观测方程这一时刻的后验也是下一时刻的先验
-
贝叶斯滤波算法推导
基础:
- 原料: { X k = f ( X k − 1 ) + Q k 随 机 过 程 Y k = h ( X k ) + R k 观 测 \left\{\begin{array}{l} \begin{aligned} X_k &= f(X_{k-1})+Q_k 随机过程\\ Y_k &= h(X_k)+R_{k}观测 \end{aligned} \end{array}\right. {XkYk=f(Xk−1)+Qk随机过程=h(Xk)+Rk观测,其中 X k , X k − 1 , Y k , Q k , R k X_k,X_{k-1},Y_k,Q_k,R_k Xk,Xk−1,Yk,Qk,Rk都是随机变量。
- $\star ∗ ∗ 重 要 假 设 ∗ ∗ : **重要假设**: ∗∗重要假设∗∗:X_0,Q_1,…,Q_k,R_1,…,R_k$相互独立
- 一系列观测值: y 1 , . . . y k y_1,...y_k y1,...yk
- 设初值 X 0 ∼ f 0 ( x ) , Q k ∼ f Q k ( x ) , R k ∼ f R k ( x ) X_0\sim f_0(x),Q_k\sim f_{Q_k}(x),R_k\sim f_{R_k}(x) X0∼f0(x),Qk∼fQk(x),Rk∼fRk(x)
- 重要定理:条件概率里的条件可以做逻辑推导 P ( X = 1 ∣ Y = 2 , Z = 3 ) = P ( X + Y = 3 ∣ Y = 2 , Z = 3 ) = P ( X + Y = 3 ∣ Y = 2 , Z − Y = 1 ) P(X=1|Y=2,Z=3)=P(X+Y=3|Y=2,Z=3)=P(X+Y=3|Y=2,Z-Y=1) P(X=1∣Y=2,Z=3)=P(X+Y=3∣Y=2,Z=3)=P(X+Y=3∣Y=2,Z−Y=1)
预测步: f 1 − ( x ) = ∫ − ∞ ∞ f Q 1 [ x − f ( v ) ] f 0 ( v ) d v f^-_1(x)=\int _{-\infin} ^\infin f_{Q_1}[x-f(v)]f_0(v)dv f1−(x)=∫−∞∞fQ1[x−f(v)]f0(v)dv
更新步: f 1 + ( x ) = η f R 1 [ y 1 − h ( x ) ] f 1 − ( x ) f_1^+(x)=\eta f_{R_1}[y_1-h(x)]f_1^-(x) f1+(x)=ηfR1[y1−h(x)]f1−(x)
总结:
f 0 ( x ) → f 1 − ( x ) = ∫ − ∞ ∞ f Q 1 [ x − f ( v ) ] f 0 ( v ) d v → f 1 + ( x ) = η f R 1 [ y 1 − h ( x ) ] f 1 − ( x ) → f 2 − ( x ) = ∫ − ∞ ∞ f Q 2 [ x − f ( v ) ] f 1 + ( v ) d v → f 2 + ( x ) = η f R 2 [ y 2 − h ( x ) ] f 1 − ( x ) → f 2 − ( x ) → . . . f_0(x) \to f^-_1(x)=\int _{-\infin} ^\infin f_{Q_1}[x-f(v)]f_0(v)dv \to f_1^+(x)=\eta f_{R_1}[y_1-h(x)]f_1^-(x)\to f^-_2(x)=\int _{-\infin} ^\infin f_{Q_2}[x-f(v)]f_1^+(v)dv \to f_2^+(x)=\eta f_{R_2}[y_2-h(x)]f_1^-(x)\to f^-_2(x)\to ... f0(x)→f1−(x)=∫−∞∞fQ1[x−f(v)]f0(v)dv→f1+(x)=ηfR1[y1−h(x)]f1−(x)→f2−(x)=∫−∞∞fQ2[x−f(v)]f1+(v)dv→f2+(x)=ηfR2[y2−h(x)]f1−(x)→f2−(x)→...,其中 η \eta η不相等,但是不影响。
f → X k = f ( X k − 1 ) + Q k f \to X_k=f(X_{k-1})+Q_k f→Xk=f(Xk−1)+Qk, f 0 ( x ) f_0(x) f0(x)是初始猜的pdf
f 1 − , f 2 − f_1^-,f_2^- f1−,f2−是预测步的pdf,先验概率
f 1 + , f 2 + f_1^+,f_2^+ f1+,f2+是更新步的pdf,后验概率也是下一步的先验概率
f Q k , f R k f_{Q_k},f_{R_k} fQk,fRk是预测与观测误差的pdf
可证: Q k Q_k Qk与 X k − 1 X_{k-1} Xk−1独立, X k X_k Xk与 R k R_k Rk独立
-
贝叶斯滤波完整算法:
- 设初值 X 0 X_0 X0的pdf f 0 ( x ) f_0(x) f0(x)
- 预测步: f k − ( x ) = ∫ − ∞ ∞ f Q k [ x − f ( v ) ] f k − 1 + ( v ) d v f_k^-(x)=\int_{-\infin}^\infin f_{Q_k}[x-f(v)]f^+_{k-1}(v)dv fk−(x)=∫−∞∞fQk[x−f(v)]fk−1+(v)dv
- 更新步: f k + ( x ) = η k f R k [ y k − h ( x ) ] f k − ( x ) f_k^+(x)=\eta_k f_{R_k}[y_k-h(x)]f^-_k(x) fk+(x)=ηkfRk[yk−h(x)]fk−(x), η k = ( ∫ − ∞ ∞ f R k [ y k − h ( x ) ] f k − ( x ) d x ) − 1 \eta_k = (\int_{-\infin}^\infin f_{R_k}[y_k-h(x)]f_k^-(x)dx)^{-1} ηk=(∫−∞∞fRk[yk−h(x)]fk−(x)dx)−1
- 对后验概率 f k + ( x ) f^+_k(x) fk+(x)分布估计期望: X ^ k + = ∫ − ∞ ∞ x f k + ( x ) d x \hat X_k^+ =\int_{-\infin}^\infin xf_k^+(x)dx X^k+=∫−∞∞xfk+(x)dx
缺点:算 η \eta η,算期望涉及无穷积分,大多数情况下无解析解。
解决:
-
做假设
- 假设 f ( x k − 1 ) , h ( x k ) f(x_{k-1}),h(x_k) f(xk−1),h(xk)为线性, Q k , R k Q_k,R_k Qk,Rk是正态分布 ⇒ \Rightarrow ⇒ Kalman Filter, KF
- 假设 f ( x k − 1 ) , h ( x k ) f(x_{k-1}),h(x_k) f(xk−1),h(xk)非线性, Q k , R k Q_k,R_k Qk,Rk是正态分布 ⇒ \Rightarrow ⇒ EKF,UKF
-
直接对无穷积分做数值积分
- 高斯积分
- 蒙特卡洛Monte Karlo 积分,PF
- 直方图滤波
第四部分:卡尔曼滤波KF
-
贝叶斯滤波基础上做假设:
- 假设 f ( x k − 1 ) , h ( x k ) f(x_{k-1}),h(x_k) f(xk−1),h(xk)为线性, f ( X k − 1 ) = F ⋅ X k − 1 , h ( X k ) = h ⋅ X k f(X_{k-1})=F\cdot X_{k-1},h(X_k)=h\cdot X_k f(Xk−1)=F⋅Xk−1,h(Xk)=h⋅Xk
- Q k , R k Q_k,R_k Qk,Rk是正态分布, Q ∼ N ( 0 , Q ) , R ∼ N ( 0 , R ) Q\sim N(0,Q),R\sim N(0,R) Q∼N(0,Q),R∼N(0,R)
-
卡尔曼滤波流程:
-
设 X k − 1 ∼ N ( μ k − 1 + , σ k − 1 + ) X_{k-1}\sim N(\mu_{k-1}^+,\sigma_{k-1}^+) Xk−1∼N(μk−1+,σk−1+)
-
预测步:
f k − ( x ) = ∫ − ∞ ∞ f Q k [ x − f ( v ) ] f k − 1 + ( v ) d v = ∫ − ∞ ∞ ( 2 π Q ) − 1 / 2 e − ( x − F ⋅ v ) 2 Q ⋅ ( 2 π σ k − 1 + ) − 1 / 2 e − ( v − μ k − 1 + ) 2 2 σ k − 1 + d v ⇒ X k − ∼ N ( F μ k − 1 + , F 2 σ k − 1 + Q ) = 记 为 N ( μ k − , σ k − ) f_k^-(x)=\int_{-\infin}^\infin f_{Q_k}[x-f(v)]f^+_{k-1}(v)dv \\=\int_{-\infin}^\infin (2\pi Q)^{-1/2}e^{-\frac{(x-F\cdot v)}{2Q}}\cdot (2\pi\sigma_{k-1}^+)^{-1/2}e^{-\frac{(v-\mu_{k-1}^+)^2}{2\sigma_{k-1}^+}}dv\\ \Rightarrow X_k^-\sim N(F\mu^+_{k-1},F^2\sigma_{k-1}+Q)\mathop{=}\limits^{记为}N(\mu_k^-,\sigma^-_k) fk−(x)=∫−∞∞fQk[x−f(v)]fk−1+(v)dv=∫−∞∞(2πQ)−1/2e−2Q(x−F⋅v)⋅(2πσk−1+)−1/2e−2σk−1+(v−μk−1+)2dv⇒Xk−∼N(Fμk−1+,F2σk−1+Q)=记为N(μk−,σk−)
解法:卷积、傅里叶变换 N ( μ , σ 2 ) → F . T e i μ t − σ 2 t 2 / 2 N(\mu,\sigma^2) \mathop{\to} \limits^{F.T} e^{i\mu t-\sigma^2t^2/2} N(μ,σ2)→F.Teiμt−σ2t2/2对比可得到与上一步之间的关系:
- μ k − = F μ k − 1 + \mu_k^-=F\mu_{k-1}^+ μk−=Fμk−1+ (1)
- σ k − = F 2 σ k − 1 + + Q \sigma_{k}^-=F^2\sigma_{k-1}^++Q σk−=F2σk−1++Q (2)
-
更新步:
f k + ( x ) = η k f R k [ y k − h ( x ) ] f k − ( x ) = η ( 2 π R ) − 1 / 2 e − ( y k − h x ) 2 2 R ⋅ ( 2 π σ k − ) − 1 / 2 e − ( x − μ k − ) 2 2 σ k − ⇒ X k + ∼ N ( h σ k − y k + R μ k − h 2 σ k − + R , R σ k − h 2 σ k − + R ) = 记 为 N ( μ k + , σ k + ) η = ( ∫ − ∞ ∞ ( 2 π R ) − 1 / 2 e − ( y k − h x ) 2 2 R ⋅ ( 2 π σ k − ) − 1 / 2 e − ( x − μ k − ) 2 2 σ k − ) − 1 f_k^+(x)=\eta_k f_{R_k}[y_k-h(x)]f^-_k(x)\\ =\eta(2\pi R)^{-1/2}e^{-\frac{(y_k-hx)^2}{2R}} \cdot (2\pi\sigma_k^-)^{-1/2}e^{-\frac{(x-\mu_k^-)^2}{2\sigma_k^-}}\\ \Rightarrow X_k^+\sim N(\frac{h\sigma_k^-y_k+R\mu_k^-}{h^2 \sigma_k^-+R},\frac{R\sigma_k^-}{h^2\sigma_k^-+R}) \mathop{=} \limits^{记为} N(\mu_k^+,\sigma_k^+)\\ \eta = (\int_{-\infin}^\infin (2\pi R)^{-1/2}e^{-\frac{(y_k-hx)^2}{2R}} \cdot (2\pi\sigma_k^-)^{-1/2}e^{-\frac{(x-\mu_k^-)^2}{2\sigma_k^-}})^{-1} fk+(x)=ηkfRk[yk−h(x)]fk−(x)=η(2πR)−1/2e−2R(yk−hx)2⋅(2πσk−)−1/2e−2σk−(x−μk−)2⇒Xk+∼N(h2σk−+Rhσk−yk+Rμk−,h2σk−+RRσk−)=记为N(μk+,σk+)η=(∫−∞∞(2πR)−1/2e−2R(yk−hx)2⋅(2πσk−)−1/2e−2σk−(x−μk−)2)−1
对比可得到与上一步之间的关系:
-
-
μ k + = h σ k − h 2 σ k − + R ( y k − h μ k − ) + μ k − = μ k − + K ( y k − h μ k − ) \mu_k^+= \frac{h\sigma_k^-}{h^2\sigma_k^- +R}(y_k-h\mu_k^-)+\mu_k^- = \mu_k^- +K(y_k-h\mu_k^-) μk+=h2σk−+Rhσk−(yk−hμk−)+μk−=μk−+K(yk−hμk−) (3)
-
σ k − = ( 1 − h 2 σ k − h 2 σ k − + R ) σ k − = ( 1 − K h ) σ k − \sigma_k^-=(1-\frac{h^2\sigma^-_k}{h^2\sigma_k^-+R})\sigma_k^-=(1-Kh)\sigma_k^- σk−=(1−h2σk−+Rh2σk−)σk−=(1−Kh)σk− (4)
-
简化(3)(4),设卡尔曼增益 K = h σ k − h 2 σ k − + R K=\frac{h\sigma^-_k}{h^2\sigma_k^-+R} K=h2σk−+Rhσk− (5)
当 R ≫ σ k − R \gg \sigma _k^- R≫σk−时, K → 0 , μ k + = μ k − K \to 0, \mu_k^+=\mu_k^- K→0,μk+=μk−,即相信预测,观测误差太大观测远远不可信。
当 R ≪ σ k − R \ll \sigma _k^- R≪σk−, K → 1 h , μ k + = y k h ⇒ y k = h x k + R K\to \frac{1}{h},\mu_k^+=\frac{y_k}{h} \Rightarrow y_k=hx_k+R K→h1,μk+=hyk⇒yk=hxk+R,即相信观测,观测误差极小。
-
矩阵形式的卡尔曼滤波: μ k → μ ⃗ k \mu_k \to \vec \mu_k μk→μk,均值向量; σ k → Σ k \sigma_k\to\Sigma_k σk→Σk协方差矩阵,F为矩阵,H为矩阵,Q,R为协方差矩阵, F 2 σ → F σ F T F^2\sigma \to F\sigma F^T F2σ→FσFT
- μ ⃗ k − = F μ ⃗ k − 1 + \vec \mu_k^- =F \vec \mu_{k-1}^+ μk−=Fμk−1+
- Σ ⃗ k − = F Σ k − 1 + F T + Q \vec \Sigma_k^- = F\Sigma_{k-1}^+ F^T+Q Σk−=FΣk−1+FT+Q
- K = Σ k − H T ( H Σ k − H T + R ) − 1 K = \Sigma_k^- H^T(H\Sigma_k^-H^T+R)^{-1} K=Σk−HT(HΣk−HT+R)−1
- μ ⃗ k + = μ ⃗ k − + K ( y ⃗ k − H μ k − ) \vec \mu_k^+ = \vec \mu_k^- +K(\vec y_k-H\mu_k^-) μk+=μk−+K(yk−Hμk−)
- Σ k + = ( I − K H ) Σ k − \Sigma_k^+=(I-KH)\Sigma_k^- Σk+=(I−KH)Σk−
-
马尔科夫性和观测独立性
证明:若 { X k = f ( X k − 1 ) + Q k 预 测 Y k = h ( X k ) + R k 观 测 \left\{\begin{array}{l} \begin{aligned} X_k &= f(X_{k-1})+Q_k 预测\\ Y_k &= h(X_k)+R_{k}观测 \end{aligned} \end{array}\right. {XkYk=f(Xk−1)+Qk预测=h(Xk)+Rk观测, X 0 , Q 1 , . . . , Q k , R 1 , . . . , R k X_0,Q_1,...,Q_k,R_1,...,R_k X0,Q1,...,Qk,R1,...,Rk相互独立,则
- 马尔科夫性: P ( X k < x k ∣ X k − 1 = x k − 1 , X k − 2 = x k − 2 , . . . , X 0 = x 0 ) = P ( X k = x k ∣ X k − 1 = x k − 1 ) P(X_k<x_k|X_{k-1}=x_{k-1},X_{k-2}=x_{k-2},...,X_0=x_0)=P(X_k=x_k|X_{k-1}=x_{k-1}) P(Xk<xk∣Xk−1=xk−1,Xk−2=xk−2,...,X0=x0)=P(Xk=xk∣Xk−1=xk−1),只与上一时刻状态有关。
- 观测独立性: P ( Y k = y k ∣ X k = x k , X k − 1 = x k − 1 , . . . , X 0 = x 0 ) = P ( Y k = y k ∣ X k = x k ) P(Y_k=y_k|X_k=x_k,X_{k-1}=x_{k-1},...,X_0=x_0)=P(Y_k=y_k|X_k=x_k) P(Yk=yk∣Xk=xk,Xk−1=xk−1,...,X0=x0)=P(Yk=yk∣Xk=xk),观测只与此刻状态有关。
-
卡尔曼滤波的应用——编程:
- 有一个带噪声的信号,用KF滤波。
- 传感器融合:已知 X = t 2 X=t^2 X=t2为信号,有两个不同的传感器对X进行观测,产生了 y a 1 , . . . , y a k , y b 1 , . . . , y b k y_{a1},...,y_{ak},y_{b1},...,y_{bk} ya1,...,yak,yb1,...,ybk,观测方程 Y a k = X k + R a k , Y b k = X k + R b k Y_{ak}=X_k+R_{ak},Y_{bk}=X_k+R_{bk} Yak=Xk+Rak,Ybk=Xk+Rbk,其中 R a k ∼ N ( 0 , 1 ) , R b k ∼ N ( 0 , 2 ) R_{ak}\sim N(0,1),R_{bk} \sim N(0,2) Rak∼N(0,1),Rbk∼N(0,2),求传感器融合后的 X ^ + = ∫ − ∞ ∞ x f + ( x ) d x \hat X^+=\int _{-\infin}^\infin xf^+(x)dx X^+=∫−∞∞xf+(x)dx