卡尔曼滤波器的数学推导

前言：这可能是网上最数学化的推导了，相较于其他的推导方法在数学上更加完备，这意味着对于其他的非线性滤波器如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\left\{-\frac{1}{2}(x_{t-1}-{\mu }_{t-1}{)}^T{\Sigma }_{t-1}^{-1}(x_{t-1}-{\mu }_{t-1})\right\}$$
仅考虑指数部分，记为 b e l ‾ ( x t ) = η ∫ e x p { − L t } d x t − 1 \overline{bel}(x_t)=\eta \int exp\{-L_t \}dx_{t-1} bel(xt​)=η∫exp{−Lt​}dxt−1​,其中
$$L_t=\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)+\frac{1}{2}(x_{t-1}-{\mu }_{t-1}{)}^T{\Sigma }_{t-1}^{-1}(x_{t-1}-{\mu }_{t-1})$$
对于$x_{t-1}$的积分的计算则考虑到概率分布在整个空间中积分为常数，由于$L_t$是关于$x_{t-1}的二次型，因此关于其的分布为高斯分布。高斯分布的计算只需要求一阶导数为零即可得到均值，求二阶导数得到方差，因此求导如下：

$$\frac{\partial L_t}{\partial x_{t-1}}=-A_t^T{R}_t^{-1}(x_t-A_t{x}_{t-1}-B_t{u}_t)+{\Sigma }_{t-1}^{-1}(x_{t-1}-{\mu }_{t-1})$$
$$\frac{{\partial }^2{L}_t}{\partial x_{t-1}^2}=A_t^T{R}_t^{-1}{A}_t+{\Sigma }_{t-1}^{-1}=:{\Psi }_t^{-1}$$
令一阶导数为零有
$$\begin{gathered} A_t^T{R}_t^{-1}(x_t-A_t{x}_{t-1}-B_t{u}_t)={\Sigma }_{t-1}^{-1}(x_{t-1}-{\mu }_{t-1}) \\ {A}_t^T{R}_t^{-1}{x}_t-A_t^T{R}_t^{-1}{A}_t{x}_{t-1}-A_t^T{R}_t^{-1}{B}_t{u}_t={\Sigma }_{t-1}^{-1}{x}_{t-1}-{\Sigma }_{t-1}^{-1}{\mu }_{t-1} \\ {A}_t^T{R}_t^{-1}{x}_t-A_t^T{R}_t^{-1}{B}_t{u}_t+{\Sigma }_{t-1}^{-1}{\mu }_{t-1}={\Sigma }_{t-1}^{-1}{x}_{t-1}+A_t^T{R}_t^{-1}{A}_t{x}_{t-1} \\ {x}_{t-1}=\frac{A_t^T{R}_t^{-1}{x}_t-A_t^T{R}_t^{-1}{B}_t{u}_t+{\Sigma }_{t-1}^{-1}{\mu }_{t-1}}{{\Sigma }_{t-1}^{-1}+A_t^T{R}_t^{-1}{A}_t} \end{gathered}$$
定义二次型函数如下：

$$L_t(x_{t-1},x_t)=\frac{1}{2}(x_{t-1}-\frac{A_t^T{R}_t^{-1}{x}_t-A_t^T{R}_t^{-1}{B}_t{u}_t+{\Sigma }_{t-1}^{-1}{\mu }_{t-1}}{{\Sigma }_{t-1}^{-1}+A_t^T{R}_t^{-1}{A}_t}{)}^T{\Psi }^{-1}(x_{t-1}-\frac{A_t^T{R}_t^{-1}{x}_t-A_t^T{R}_t^{-1}{B}_t{u}_t+{\Sigma }_{t-1}^{-1}{\mu }_{t-1}}{{\Sigma }_{t-1}^{-1}+A_t^T{R}_t^{-1}{A}_t})$$
有 d e t ( 2 π Ψ t ) 1 2 ∫ e x p { L t ( x t , x t − 1 ) } d x t − 1 = 1 det(2\pi\Psi_t)^{\frac{1}{2}}\int exp\{L_t(x_t,x_{t-1})\}dx_{t-1}=1 det(2πΨt​)21​∫exp{Lt​(xt​,xt−1​)}dxt−1​=1,因此只要将原式转换为$L_t=L_t(x_t,x_{t-1})+L_t(x_t)$即可将积分去掉。
b e l ‾ ( x t ) = η ∫ e x p { − L t } d x t − 1 = η ∫ e x p { − L t ( x t ) − L t ( x t , x t − 1 ) } d x t − 1 = η e x p { − L t ( x t ) } ∫ e x p { − L t ( x t , x t − 1 ) } d x t − 1 = η e x p { − L t ( x t ) \overline{bel}(x_t)=\eta \int exp\{-L_t \}dx_{t-1}\\ =\eta \int exp\{-L_t(x_t) -L_t(x_t,x_{t-1}) \}dx_{t-1}\\ =\eta exp\{-L_t(x_t) \}\int exp\{-L_t(x_t,x_{t-1}) \}dx_{t-1}\\ =\eta exp\{-L_t(x_t) \\ bel(xt​)=η∫exp{−Lt​}dxt−1​=η∫exp{−Lt​(xt​)−Lt​(xt​,xt−1​)}dxt−1​=ηexp{−Lt​(xt​)}∫exp{−Lt​(xt​,xt−1​)}dxt−1​=ηexp{−Lt​(xt​)
因此只需要考虑$L_t(x_t)$的分布即可，由于对于$x_t$是二次型，为高斯分布，可以采用求一阶导数和二阶导数的方法求得其均值和方差。
$$\begin{gathered} L_t(x_t)=L_t-L_t(x_t,x_{t-1}) \\ =\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)+\frac{1}{2}(x_{t-1}-{\mu }_{t-1}{)}^T{\Sigma }_{t-1}^{-1}(x_{t-1}-{\mu }_{t-1})-\frac{1}{2}(x_{t-1}-\frac{A_t^T{R}_t^{-1}{x}_t-A_t^T{R}_t^{-1}{B}_t{u}_t+{\Sigma }_{t-1}^{-1}{\mu }_{t-1}}{{\Sigma }_{t-1}^{-1}+A_t^T{R}_t^{-1}{A}_t}{)}^T{\Psi }^{-1}(x_{t-1}-\frac{A_t^T{R}_t^{-1}{x}_t-A_t^T{R}_t^{-1}{B}_t{u}_t+{\Sigma }_{t-1}^{-1}{\mu }_{t-1}}{{\Sigma }_{t-1}^{-1}+A_t^T{R}_t^{-1}{A}_t}) \end{gathered}$$
由于该式子的化简过于繁琐，就不化简直接求导数

一阶导数：
$$\begin{gathered} \frac{\partial L_t(x_t)}{\partial x_t}=R_t^{-1}(x_t-A_t{x}_{t-1}-B_t{u}_t)+\frac{R_t^{-1}{A}_t}{{\Sigma }_{t-1}^{-1}+A_t^T{R}_t^{-1}{A}_t}{\Psi }^{-1}(x_{t-1}-\frac{A_t^T{R}_t^{-1}{x}_t-A_t^T{R}_t^{-1}{B}_t{u}_t+{\Sigma }_{t-1}^{-1}{\mu }_{t-1}}{{\Sigma }_{t-1}^{-1}+A_t^T{R}_t^{-1}{A}_t}) \\ =R_t^{-1}(x_t-A_t{x}_{t-1}-B_t{u}_t)+\frac{R_t^{-1}{A}_t}{{\Sigma }_{t-1}^{-1}+A_t^T{R}_t^{-1}{A}_t}(A_t^T{R}_t^{-1}{A}_t+{\Sigma }_{t-1}^{-1})(x_{t-1}-\frac{A_t^T{R}_t^{-1}{x}_t-A_t^T{R}_t^{-1}{B}_t{u}_t+{\Sigma }_{t-1}^{-1}{\mu }_{t-1}}{{\Sigma }_{t-1}^{-1}+A_t^T{R}_t^{-1}{A}_t}) \\ =R_t^{-1}(x_t-A_t{x}_{t-1}-B_t{u}_t)+R_t^{-1}{A}_t(x_{t-1}-\frac{A_t^T{R}_t^{-1}{x}_t-A_t^T{R}_t^{-1}{B}_t{u}_t+{\Sigma }_{t-1}^{-1}{\mu }_{t-1}}{{\Sigma }_{t-1}^{-1}+A_t^T{R}_t^{-1}{A}_t}) \\ =R_t^{-1}(x_t-B_t{u}_t)-R_t^{-1}{A}_t\frac{A_t^T{R}_t^{-1}{x}_t-A_t^T{R}_t^{-1}{B}_t{u}_t+{\Sigma }_{t-1}^{-1}{\mu }_{t-1}}{{\Sigma }_{t-1}^{-1}+A_t^T{R}_t^{-1}{A}_t} \\ =R_t^{-1}(x_t-B_t{u}_t)-R_t^{-1}{A}_t\frac{A_t^T{R}_t^{-1}(x_t-B_t{u}_t)+{\Sigma }_{t-1}^{-1}{\mu }_{t-1}}{{\Sigma }_{t-1}^{-1}+A_t^T{R}_t^{-1}{A}_t} \\ =[R_t^{-1}-R_t^{-1}{A}_t^T\frac{A_t{R}_t^{-1}}{{\Sigma }_{t-1}^{-1}+A_t^T{R}_t^{-1}{A}_t}](x_t-B_t{u}_t)-R_t^{-1}{A}_t\frac{{\Sigma }_{t-1}^{-1}{\mu }_{t-1}}{{\Sigma }_{t-1}^{-1}+A_t^T{R}_t^{-1}{A}_t}(求逆定理) \\ =(R_t+A_t{\Sigma }_{t-1}{A}_t^T{)}^{-1}(x_t-B_t{u}_t)-R_t^{-1}{A}_t\frac{{\Sigma }_{t-1}^{-1}{\mu }_{t-1}}{{\Sigma }_{t-1}^{-1}+A_t^T{R}_t^{-1}{A}_t} \end{gathered}$$

二阶导数：
$$\frac{{\partial }^2{L}_t(x_t)}{\partial x_t^2}=(R_t+A_t{\Sigma }_{t-1}{A}_t^T{)}^{-1}$$

当一阶导数为零时
$$\begin{gathered} 0=(R_t+A_t{\Sigma }_{t-1}{A}_t^T{)}^{-1}(x_t-B_t{u}_t)-R_t^{-1}{A}_t\frac{{\Sigma }_{t-1}^{-1}{\mu }_{t-1}}{{\Sigma }_{t-1}^{-1}+A_t^T{R}_t^{-1}{A}_t} \\ (R_t+A_t{\Sigma }_{t-1}{A}_t^T{)}^{-1}(x_t-B_t{u}_t)=R_t^{-1}{A}_t\frac{{\Sigma }_{t-1}^{-1}{\mu }_{t-1}}{{\Sigma }_{t-1}^{-1}+A_t^T{R}_t^{-1}{A}_t} \\ (x_t-B_t{u}_t)=(R_t+A_t{\Sigma }_{t-1}{A}_t^T)R_t^{-1}{A}_t({\Sigma }_{t-1}^{-1}+A_t^T{R}_t^{-1}{A}_t{)}^{-1}{\Sigma }_{t-1}^{-1}{\mu }_{t-1} \\ (x_t-B_t{u}_t)=(R_t+A_t{\Sigma }_{t-1}{A}_t^T)R_t^{-1}{A}_t(I+{\Sigma }_{t-1}{A}_t^T{R}_t^{-1}{A}_t{)}^{-1}{\mu }_{t-1} \\ (x_t-B_t{u}_t)=(A_t+A_t{\Sigma }_{t-1}{A}_t^T{R}_t^{-1}{A}_t)(I+{\Sigma }_{t-1}{A}_t^T{R}_t^{-1}{A}_t{)}^{-1}{\mu }_{t-1} \\ (x_t-B_t{u}_t)=A_t(I+{\Sigma }_{t-1}{A}_t^T{R}_t^{-1}{A}_t)(I+{\Sigma }_{t-1}{A}_t^T{R}_t^{-1}{A}_t{)}^{-1}{\mu }_{t-1} \\ (x_t-B_t{u}_t)=A_t{\mu }_{t-1} \\ {x}_t=A_t{\mu }_{t-1}+B_t{u}_t \end{gathered}$$

后验

根据贝叶斯滤波器的后验公式
$$\mathrm{bel}\left(x_t\right)=\eta \underset{\sim \mathcal{N}\left(z_t:C_t{x}_t,Q_t\right)}{\underbrace{p\left(z_t|x_t\right)}}\underset{\sim \mathcal{N}\left(x_t:{\bar{\mu }}_t,{\bar{\Sigma }}_t\right)}{\underbrace{\overline{\mathrm{bel}}\left(x_t\right)}}$$

简化表示为 b e l ( x t ) = η e x p { − J t } bel(x_t)=\eta exp\{-J_t\} bel(xt​)=ηexp{−Jt​},其中
$$J_t=\frac{1}{2}(z_t-C_t{x}_t{)}^T{Q}^{-1}(z_t-C_t{x}_t)+\frac{1}{2}(x_t-{\overline{\mu }}_t{)}^T{\overline{\Sigma }}_t^{-1}(x_t-{\overline{\mu }}_t)$$

由于依旧是$x_t$的二次型，即满足高斯分布，同样可以采用求一阶导数和二阶导数的方法求均值和方差。

一阶导数：
$$\frac{\partial J}{\partial x_t}=-C_t^T{Q}_t^{-1}(z_t-C_t{x}_t)+{\overline{\Sigma }}_t^{-1}(x_t-{\overline{\mu }}_t)$$

二阶导数：

$$\frac{{\partial }^{2}J}{\partial x_t^2}=C_t^T{Q}_t^{-1}{C}_t+{\overline{\Sigma }}_t^{-1}$$

因此，${\Sigma }_t=(C_t^T{Q}_t^{-1}{C}_t+{\overline{\Sigma }}_t^{-1}{)}^{-1}$

令一阶导数为零
$$\begin{gathered} 0=-C_t^T{Q}_t^{-1}(z_t-C_t{x}_t)+{\overline{\Sigma }}_t^{-1}(x_t-{\overline{\mu }}_t) \\ {C}_t^T{Q}_t^{-1}(z_t-C_t{x}_t)={\overline{\Sigma }}_t^{-1}(x_t-{\overline{\mu }}_t) \\ {C}_t^T{Q}_t^{-1}{z}_t-C_t^T{Q}_t^{-1}{C}_t{x}_t={\overline{\Sigma }}_t^{-1}{x}_t-{\overline{\Sigma }}_t^{-1}{\overline{\mu }}_t \\ (C_t^T{Q}_t^{-1}{C}_t+{\overline{\Sigma }}_t^{-1})x_t=C_t^T{Q}_t^{-1}{z}_t+{\overline{\Sigma }}_t^{-1}{\overline{\mu }}_t \\ {x}_t=(C_t^T{Q}_t^{-1}{C}_t+{\overline{\Sigma }}_t^{-1}{)}^{-1}{C}_t^T{Q}_t^{-1}{z}_t+(C_t^T{Q}_t^{-1}{C}_t+{\overline{\Sigma }}_t^{-1}{)}^{-1}{\overline{\Sigma }}_t^{-1}{\overline{\mu }}_t \end{gathered}$$
令$x_t={\overline{\mu }}_t+K_k(z_t-C_t{\overline{\mu }}_t)$,整理得
$$K_k=C_t^T{Q}_t^{-1}(C_t^T{Q}_t^{-1}{C}_t+{\overline{\Sigma }}_t^{-1}{)}^{-1}$$
最后将${\Sigma }_t$也用$K_k$来表达，

$$\begin{gathered} {\Sigma }_t=(C_t^T{Q}_t^{-1}{C}_t+{\overline{\Sigma }}_t^{-1}{)}^{-1} \\ {\Sigma }_t=(I-C_t^T{Q}_t^{-1}(C_t^T{Q}_t^{-1}{C}_t+{\overline{\Sigma }}_t^{-1}{)}^{-1}{C}_t){\overline{\Sigma }}_t \\ {\Sigma }_t=(I-K_k{C}_t){\overline{\Sigma }}_t \end{gathered}$$
综上，卡尔曼滤波器的五个公式便推导完毕了。