本文从概率论中的贝叶斯准则和全概率公式出发，详细推导了卡尔曼滤波器的数学模型，包括先验和后验概率的计算，以及卡尔曼增益的推导。
前言：这可能是网上最数学化的推导了，相较于其他的推导方法在数学上更加完备，这意味着对于其他的非线性滤波器如EKF或者信息滤波器IF很多其他地方都可以采用同样的方法进行推导，一通百通。本文的卡尔曼滤波器的推导从最基础的概率论中的贝叶斯准则和全概率公式推起，一步一步的完成卡尔曼滤波器，适合对卡尔曼有一定概念上的理解希望深入学习滤波器方法的同学。
卡尔曼滤波算法
μˉt=Atμt−1+Btut \bar{\mu}_t = A_t \mu_{t-1} + B_t u_t μˉt=Atμt−1+Btut
Σˉt=AtΣt−1AtT+Rt \bar{\Sigma}_t = A_t \Sigma_{t-1} A_t^T + R_t Σˉt=AtΣt−1AtT+Rt
Kt=ΣˉtCtT(CtΣˉtCtT+Qt)−1 K_t = \bar{\Sigma}_t C_t^T(C_t \bar{\Sigma}_t C_t^T + Q_t)^{-1} Kt=ΣˉtCtT(CtΣˉtCtT+Qt)−1
μt=μˉt+Kt(zt−Ctμˉt) \mu_t = \bar{\mu}_t + K_t (z_t-C_t \bar{\mu}_t) μt=μˉt+Kt(zt−Ctμˉt)
Σt=(I−KtCt)Σˉt \Sigma_t = (I-K_t C_t)\bar{\Sigma}_t Σt=(I−KtCt)Σˉt
推导过程：
先验条件
贝叶斯准则：
p(x∣y,z)=p(y∣x,z)p(x∣z)p(y∣z) p(x|y,z)=\frac{p(y|x,z)p(x|z)}{p(y|z)} p(x∣y,z)=p(y∣z)p(y∣x,z)p(x∣z)
全概率公式：
p(x)=∫p(x∣y)p(y)dy p(x) = \int p(x|y)p(y) dy p(x)=∫p(x∣y)p(y)dy
马尔可夫性：认为xt−1x_{t-1}xt−1是zt−1z_{t-1}zt−1、ut−1u_{t-1}ut−1的最优估计
贝叶斯滤波器推导
p(xt∣z1:t,u1:t)=p(xt∣zt,z1:t−1,u1:t)(套用贝叶斯准则)=p(zt∣xt,z1:t−1,u1:t)p(xt∣z1:t−1,u1:t)p(zt∣z1:t−1,u1:t)(分母与xt无关)=ηp(zt∣xt,z1:t−1,u1:t)p(xt∣z1:t−1,u1:t)(马尔可夫性)=ηp(zt∣xt)p(xt∣z1:t−1,u1:t)(区分先验后验) p(x_t|z_{1:t},u_{1:t})=p(x_t|z_t,z_{1:t-1},u_{1:t}) (套用贝叶斯准则) \\ =\frac{p(z_t|x_t,z_{1:t-1},u_{1:t})p(x_t|z_{1:t-1},u_{1:t})}{p(z_t|z_{1:t-1},u_{1:t})}(分母与x_t无关)\\ =\eta p(z_t|x_t,z_{1:t-1},u_{1:t})p(x_t|z_{1:t-1},u_{1:t}) (马尔可夫性)\\ =\eta p(z_t|x_t)p(x_t|z_{1:t-1},u_{1:t}) (区分先验后验) \\ p(xt∣z1:t,u1:t)=p(xt∣zt,z1:t−1,u1:t)(套用贝叶斯准则)=p(zt∣z1:t−1,u1:t)p(zt∣xt,z1:t−1,u1:t)p(xt∣z1:t−1,u1:t)(分母与xt无关)=ηp(zt∣xt,z1:t−1,u1:t)p(xt∣z1:t−1,u1:t)(马尔可夫性)=ηp(zt∣xt)p(xt∣z1:t−1,u1:t)(区分先验后验)
对于先验分布p(xt∣z1:t−1,u1:t)p(x_t|z_{1:t-1},u_{1:t})p(xt∣z1:t−1,u1:t)带入全概率公式
bel‾(xt)=p(xt∣z1:t−1,u1:t)=∫p(xt∣xt−1,z1:t−1,u1:t)p(xt−1∣z1:t−1,u1:t)dxt−1 \overline{bel}(x_t) = p(x_t|z_{1:t-1},u_{1:t}) \\ = \int p(x_t|x_{t-1},z_{1:t-1},u_{1:t})p(x_{t-1}|z_{1:t-1},u_{1:t})d x_{t-1} bel(xt)=p(xt∣z1:t−1,u1:t)=∫p(xt∣xt−1,z1:t−1,u1:t)p(xt−1∣z1:t−1,u1:t)dxt−1
最终得到先验和后验公式
先验：
bel‾(xt)=∫p(xt∣xt−1,z1:t−1,u1:t)p(xt−1∣z1:t−1,u1:t)dxt−1 \overline{bel}(x_t) = \int p(x_t|x_{t-1},z_{1:t-1},u_{1:t})p(x_{t-1}|z_{1:t-1},u_{1:t})d x_{t-1} bel(xt)=∫p(xt∣xt−1,z1:t−1,u1:t)p(xt−1∣z1:t−1,u1:t)dxt−1
后验：
bel(xt)=ηp(zt∣xt)bel‾(xt) bel(x_t)=\eta p(z_t|x_t)\overline{bel}(x_t) bel(xt)=ηp(zt∣xt)bel(xt)
卡尔曼滤波器推导
根据贝叶斯滤波器带入线性的模型，推导得到卡尔曼滤波器。
模型
预测方程：
xt=Atxt−1+Btut+ϵt x_t=A_t x_{t-1} + B_t u_t + \epsilon_t xt=Atxt−1+Btut+ϵt
xt=(x1,tx2,t⋮xn,t)ut=(u1,tu2,t⋮um,t) x_{t}=\left(\begin{array}{c} x_{1, t} \\ x_{2, t} \\ \vdots \\ x_{n, t} \end{array}\right) \boldsymbol{u}_{t}=\left(\begin{array}{c} u_{1, t} \\ u_{2, t} \\ \vdots \\ u_{m, t} \end{array}\right) xt=⎝⎜⎜⎜⎛x1,tx2,t⋮xn,t⎠⎟⎟⎟⎞ut=⎝⎜⎜⎜⎛u1,tu2,t⋮um,t⎠⎟⎟⎟⎞
观测方程：
zt=Ctxt+δt z_{t}=C_{t} x_{t}+\delta_{t} zt=Ctxt+δt
预测
根据贝叶斯滤波器的先验公式
bel‾(xt)=∫p(xt∣xt−1,ut)⏟∼N(xt;Atxt−1+Btut,Rt)bel(xt−1)⏟∼N(xt−1;μt−1,Σt−1)dxt−1\overline{\mathrm{bel}}\left(x_{t}\right)=\int \underbrace{p\left(x_{t} | x_{t-1}, u_{t}\right)}_{\sim N\left(x_{t} ; A_{t} x_{t-1}+B_{t} u_{t}, R_{t}\right)} \underbrace{\mathrm{bel}\left(x_{t-1}\right)}_{\sim N\left(x_{t-1} ; \mu_{t-1}, \Sigma_{t-1}\right)} \mathrm{d} x_{t-1}bel(xt)=∫∼N(xt;Atxt−1+Btut,Rt)
p(xt∣xt−1,ut)∼N(xt−1;μt−1,Σt−1)
bel(xt−1)dxt−1
其中，t-1时刻bel(xt−1)bel(x_{t-1})bel(xt−1)的概率分布为高斯分布，均值为μt−1\mu_{t-1}μt−1，方差为Σt−1\Sigma_{t-1}Σt−1。
bel‾(xt)=η∫exp{ −12(xt−Atxt−1−Btut)TRt−1(xt−Atxt−1−Btut)}exp{ −12(xt−1−μt−1)TΣt−1−1(xt−1−μt−1)} \overline{bel}(x_t) = \eta \int exp \left \{ -\frac{1}{2} (x_t-A_t x_{t-1}-B_t u_t)^TR_{t}^{-1}(x_t-A_t x_{t-1}-B_t u_t)\right\}exp\left \{ -\frac{1}{2}(x_{t-1}-\mu_{t-1})^T \Sigma_{t-1}^{-1}(x_{t-1}-\mu_{t-1})\right\} bel(xt)=η∫exp{ −21(xt−Atxt−1−Btut)TRt−1(xt−Atxt−1−Btut)}exp{ −21(xt
最低0.47元/天 解锁文章
扫一扫
1072
到【灌水乐园】发言
