卡尔曼滤波器的数学推导

前言：这可能是网上最数学化的推导了，相较于其他的推导方法在数学上更加完备，这意味着对于其他的非线性滤波器如EKF或者信息滤波器IF很多其他地方都可以采用同样的方法进行推导，一通百通。本文的卡尔曼滤波器的推导从最基础的概率论中的贝叶斯准则和全概率公式推起，一步一步的完成卡尔曼滤波器，适合对卡尔曼有一定概念上的理解希望深入学习滤波器方法的同学。

卡尔曼滤波算法

$${\bar{\mu }}_t=A_t{\mu }_{t-1}+B_t{u}_t$$
$${\bar{\Sigma }}_t=A_t{\Sigma }_{t-1}{A}_t^T+R_t$$
$$K_t={\bar{\Sigma }}_t{C}_t^T(C_t{\bar{\Sigma }}_t{C}_t^T+Q_t{)}^{-1}$$
$${\mu }_t={\bar{\mu }}_t+K_t(z_t-C_t{\bar{\mu }}_t)$$
$${\Sigma }_t=(I-K_t{C}_t){\bar{\Sigma }}_t$$

推导过程：

先验条件

贝叶斯准则：
$$p(x|y,z)=\frac{p(y|x,z)p(x|z)}{p(y|z)}$$
全概率公式：
$$p(x)=\int p(x|y)p(y)dy$$
马尔可夫性：认为$x_{t-1}$是$z_{t-1}$、$u_{t-1}$的最优估计

贝叶斯滤波器推导

$$\begin{gathered} 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})(区分先验后验) \end{gathered}$$
对于先验分布$p(x_t|z_{1:t-1},u_{1:t})$带入全概率公式
$$\begin{gathered} \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} \end{gathered}$$
最终得到先验和后验公式

先验：
$$\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(x_t)=\eta p(z_t|x_t)\overline{bel}(x_t)$$

卡尔曼滤波器推导

根据贝叶斯滤波器带入线性的模型，推导得到卡尔曼滤波器。

模型

预测方程：
$$x_t=A_t{x}_{t-1}+B_t{u}_t+{ϵ}_t$$
$$x_t=\left(\begin{array}{c}{x}_{1,t}\\ x_{2,t}\\ ⋮\\ x_{n,t}\end{array}\right){\mathbf{u}}_t=\left(\begin{array}{c}{u}_{1,t}\\ u_{2,t}\\ ⋮\\ u_{m,t}\end{array}\right)$$

观测方程：
$$z_t=C_t{x}_t+{\delta }_t$$

预测

根据贝叶斯滤波器的先验公式
$$\overline{\mathrm{b}\mathrm{e}\mathrm{l}}\left(x_t\right)=\int \underset{\sim N\left(x_t;A_t{x}_{t-1}+B_t{u}_t,R_t\right)}{\underbrace{p\left(x_t|x_{t-1},u_t\right)}}\underset{\sim N\left(x_{t-1};{\mu }_{t-1},{\Sigma }_{t-1}\right)}{\underbrace{\mathrm{b}\mathrm{e}\mathrm{l}\left(x_{t-1}\right)}}\mathrm{d}{x}_{t-1}$$
其中，t-1时刻$bel(x_{t-1})$的概率分布为高斯分布，均值为${\mu }_{t-1}$，方差为${\Sigma }_{t-1}$。

$$\overline{bel}(x_t)=\eta \int exp\left\{-\frac{1}{2}(x_t-A_t{x}_{t-1}-B_t{u}_t{)}^T{R}_t^{-1}(x_t-A_t{x}_{t-1}-B_t{u}_t)\right\}exp\{-\frac{1}{2}(x_t$$