卡尔曼滤波器有无数种推导方法：你可以将因果IIR维纳滤波器写成递推的形式，那么就是卡尔曼滤波器；你可以将估计值测量值等等都假设为高斯分布，通过最大后验估计(maximun a posterior)得到批量最小二乘法(batch least-squares)：$(H^TW^{-1}H)x = H^TW^{-1}z$，利用$H^TW^{-1}H$的稀疏性，通过Cholesky分解并发现其中的递推规则，总结出卡尔曼滤波器。。。你也可以直接解析地求得后验分布并总结出卡尔曼滤波器，而这种推导方法是最简洁的。

为什么要将估计量和测量值建模为高斯分布？一方面因为这是现实世界的合理假设，另外高斯分布这种指数上二次型有很好的特性：两个高斯分布的和、积都是高斯分布，从而高斯分布经过线性变换之后还是高斯分布。

将参数建模为高斯分布的均值，假设只有线性变换，然后该怎么求？高斯分布的均值和最大值一样，而且它是凸的，而且指数只是二次的，于是求导令它等于0就得到结果了。求导得到的就是最小二乘法的方程，也就是说高斯分布求最值其实是最小二乘意义下的最优值。。然后接下来就要神奇地“看出”最小二乘方程里面的稀疏性并化简那些华丽的公式，才能得到卡尔曼滤波器。。

然而，利用贝叶斯公式，从联合高斯分布推导出后验分布的解析表达式，就可以得到后验的均值和方差，就是我们想要的东西了。

多维高斯分布

$$\mathcal N(\bf\mu, \bf\Sigma) \sim p(\bf x) = \frac{1}{\sqrt{(2\pi)^Ndet(\bf\Sigma)}}exp(-\frac{1}{2}(\bf x - \bf\mu)^T\Sigma^{-1}(\bf x - \bf\mu))$$

其中$\bf\mu = \bigl[ \begin{matrix} \mu_1&…&\mu_N \end{matrix} \bigr]^T$ 为N个随机变量的均值，$\bf\Sigma = E[(\bf x - \bf\mu)(\bf x - \bf\mu)^T]$为N个随机变量的协方差矩阵。

舒尔补

Schur complement将一个分4块的可逆矩阵分解为上三角×对角×下三角(UDL)，或者下三角×对角×上三角(LDU)形式：

$$\begin{aligned} \begin{bmatrix} A&B\\C&D \end{bmatrix} &= \begin{bmatrix} I& \\ CA^{-1}&I \end{bmatrix} \begin{bmatrix} A& \\ &D-CA^{-1}B \end{bmatrix} \begin{bmatrix} I&A^{-1}B \\ &I \end{bmatrix} \\ &= \begin{bmatrix} I&BD^{-1} \\ &I \end{bmatrix} \begin{bmatrix} A-BD^{-1}C& \\ &D \end{bmatrix} \begin{bmatrix} I& \\ D^{-1}C&I \end{bmatrix} \end{aligned}$$

其中$D-CA^{-1}B$和$A-BD^{-1}C$就是舒尔补，字母顺序是顺时针的，方便记忆。

求逆也很方便：

$$\begin{aligned} \begin{bmatrix} A&B\\C&D \end{bmatrix}^{-1} &= \begin{bmatrix} I&-A^{-1}B \\ &I \end{bmatrix} \begin{bmatrix} A^{-1}& \\ &(D-CA^{-1}B)^{-1} \end{bmatrix} \begin{bmatrix} I& \\ -CA^{-1}&I \end{bmatrix} \\ &= \begin{bmatrix} I& \\ -D^{-1}C&I \end{bmatrix} \begin{bmatrix} (A-BD^{-1}C)^{-1}& \\ &D^{-1} \end{bmatrix} \begin{bmatrix} I&-BD^{-1} \\ &I \end{bmatrix} \end{aligned}$$

对于协方差矩阵这种对称矩阵求舒尔补，$B=C$，随处可见有二次型。

将上面求逆的式子乘起来得到华丽的式子：

$$\begin{aligned} \begin{bmatrix} A^{-1}+A^{-1}B(D-CA^{-1}B)^{-1}CA^{-1} & -A^{-1}B(D-CA^{-1}B)^{-1} \\ -(D-CA^{-1}B)^{-1}CA^{-1} & (D-CA^{-1}B)^{-1} \end{bmatrix} \\ = \begin{bmatrix} (A-BD^{-1}C)^{-1} & -(A-BD^{-1}C)^{-1}BD^{-1} \\ -D^{-1}C(A-BD^{-1}C)^{-1} & D^{-1}+D^{-1}C(A-BD^{-1}C)^{-1}BD^{-1} \end{bmatrix} \end{aligned}$$

拆开4个式子，称为SMW等式(Sherman-Morrison-Woodbury)：

$$\begin{aligned} (A-BD^{-1}C)^{-1} &\equiv A^{-1}+A^{-1}B(D-CA^{-1}B)^{-1}CA^{-1} \\ (D-CA^{-1}B)^{-1} &\equiv D^{-1}+D^{-1}C(A-BD^{-1}C)^{-1}BD^{-1} \\ A^{-1}B(D-CA^{-1}B)^{-1} &\equiv (A-BD^{-1}C)^{-1}BD^{-1} \\ D^{-1}C(A-BD^{-1}C)^{-1} &\equiv (D-CA^{-1}B)^{-1}CA^{-1} \end{aligned}$$

其实可以看出，若ABCD分别代换为DCBA，前面两组式子是等价的，后面两组式子也是等价的，相当于只有两条等式。利用这些等式可以进行化简，两个项变一个项，左乘变右乘等等。虽然后文并没用到，但是别的卡尔曼滤波器推导方法则是反复利用这些公式来将结果化到标准形式。。

乘法原理和贝叶斯公式

一个联合概率密度函数可以写成连乘的形式；其中$\bf {x_i}$也可以是个向量：

$$p(x_1,x_2,...) = p(x_1)p(x_2|x_1)p(x_3|x_1,x_2)...$$

如果只有两个（两组）随机变量，联合概率密度函数可以写成两种形式：

$$\begin{aligned} p(x,y) &= p(x)p(y|x) \\ &= p(y)p(x|y) \end{aligned}$$

调整一下就是常见的贝叶斯公式：

$$p(y|x) = \frac{p(x|y)p(y)}{p(x)}$$

赋予它一些含义：$p(y|x)$称为后验(posterior)，$p(x|y)$称为似然(likelihood)，$p(y)$称为先验(prior)。

展开联合高斯概率密度函数

已知

$$\begin{aligned} \bf p(x) &= \bf \mathcal N(\mu_x, \Sigma_{xx}) \\ \bf p(y) &= \bf \mathcal N(\mu_y, \Sigma_{yy}) \\ \bf p(x, y) &= \mathcal N \left(\begin{bmatrix} \mu_x \\ \mu_y \end{bmatrix}, \begin{bmatrix} \bf\Sigma_{xx}&\bf\Sigma_{xy}\\\bf\Sigma_{yx}&\bf\Sigma_{yy} \end{bmatrix} \right) \\ &= \bf \eta \space exp \left(-\frac{1}{2} \begin{bmatrix} \bf x-\mu_x \\ \bf y-\mu_y \end{bmatrix}^T \begin{bmatrix} \bf\Sigma_{xx}&\bf\Sigma_{xy}\\\bf\Sigma_{yx}&\bf\Sigma_{yy} \end{bmatrix}^{-1} \begin{bmatrix} \bf x-\mu_x \\ \bf y-\mu_y \end{bmatrix} \right) \end{aligned}$$

其中$\bf\Sigma_{xy} = \bf\Sigma_{yx}^T$。我们展开联合分布$\bf p(x,y)$，只考虑指数部分的二次型：

$$\begin{aligned} &\overbrace{ \begin{bmatrix} \bf x-\mu_x \\ \bf y-\mu_y \end{bmatrix}^T \underbrace{\begin{bmatrix} \bf\Sigma_{xx}&\bf\Sigma_{xy}\\\bf\Sigma_{yx}&\bf\Sigma_{yy} \end{bmatrix}^{-1}}_\text{使用求逆的舒尔补展开} \begin{bmatrix} \bf x-\mu_x \\ \bf y-\mu_y \end{bmatrix} }^{\bf p(x,y)\text{联合概率密度}} \\ =&\underbrace{ \begin{bmatrix} \bf x-\mu_x \\ \bf y-\mu_y \end{bmatrix}^T \begin{bmatrix} I & \\ \bf-\Sigma_{yy}^{-1}\Sigma_{yx} & I \end{bmatrix} } \begin{bmatrix} \bf(\Sigma_{xx} - \Sigma_{xy}\Sigma_{yy}^{-1}\Sigma_{yx})^{-1} & \\ & \bf\Sigma_{yy}^{-1} \end{bmatrix} \underbrace{\begin{bmatrix} I & \bf-\Sigma_{xy}\Sigma_{yy}^{-1} \\ & I \end{bmatrix} \begin{bmatrix} \bf x-\mu_x \\ \bf y-\mu_y \end{bmatrix}} \\ =& \begin{bmatrix} \bf x-\mu_x-\Sigma_{xy}\Sigma_{yy}^{-1}(y-\mu_y) \\ \bf (y-\mu_y) \end{bmatrix} ^T \underbrace {\begin{bmatrix} \bf(\Sigma_{xx} - \Sigma_{xy}\Sigma_{yy}^{-1}\Sigma_{yx})^{-1} & \\ & \bf\Sigma_{yy}^{-1} \end{bmatrix} }_\text{对角块矩阵，二次型展开很方便} \begin{bmatrix} \bf x-\mu_x-\Sigma_{xy}\Sigma_{yy}^{-1}(y-\mu_y) \\ \bf (y-\mu_y) \end{bmatrix} \\ =& \underbrace{ \bf (x-\mu_x-\Sigma_{xy}\Sigma_{yy}^{-1}(y-\mu_y))^T(\Sigma_{xx} - \Sigma_{xy}\Sigma_{yy}^{-1}\Sigma_{yx})^{-1}(x-\mu_x-\Sigma_{xy}\Sigma_{yy}^{-1}(y-\mu_y)) }_{\bf p(x|y)\text{部分}} \\ &+ \underbrace{ \bf (y-\mu_y)^T\Sigma_{yy}^{-1}(y-\mu_y) }_{\bf p(y)\text{部分}} \end{aligned}$$

从而得到后验概率$\bf p(x|y)$，其中协方差矩阵是一个关于$\bf\Sigma_{xx}$的舒尔补：

$$\bf p(x | y) = \mathcal N\left( \mu_x + \Sigma_{xy}\Sigma_{yy}^{-1}( y - \mu_y), \quad \Sigma_{xx} - \Sigma_{xy}\Sigma_{yy}^{-1}\Sigma_{yx} \right)$$

同理，用另一种方式展开舒尔补，我们可以得到x的后验概率：

$$\bf p(y | x) = \mathcal N\left( \mu_y + \Sigma_{yx}\Sigma_{xx}^{-1}( x - \mu_x), \quad \Sigma_{yy} - \Sigma_{yx}\Sigma_{xx}^{-1}\Sigma_{xy} \right)$$

推导经典的5个卡尔曼滤波器公式

将系统建模为以下两个线性方程：运动方程和观测方程：

$$\begin{aligned} \begin{cases} \bf x_k = \bf A_k\bf x_{k-1} &+ \bf w_k + &\bf u_k \\ \bf y_k = \bf C_k\bf x_{k} &+ &\bf v_k \end{cases} \end{aligned}$$

其中，$\bf x_k$是当前系统的状态（比如速度、位姿等），$\bf w_k$为当前时刻系统的输入（外界驱动的因素），$\bf y_k$是当前观测（传感器测得值）；它们都加上了0均值的高斯噪声$\bf u_k \sim \mathcal N(\bf 0, \bf Q_k)$和$\bf v_k \sim \mathcal N(\bf 0, \bf R_k)$。

由运动方程看出，系统是一个由带$\bf u_k$噪声的$\bf w_k$驱动的一阶自回归系统（AR(1)模型），因此系统是一定程度上可预测的；另外系统的观测方程描述了在$\bf x_k$状态下传感器可能的读数$\bf y_k$。根据运动方程得到的是$p(\bf x_k | \bf x_{k-1})$的分布，根据观测方程则得到$p(\bf y_k | \bf x_{k-1})$的分布。我们先算得它们的联合分布$p(\bf x_k,\bf y_k | \bf x_{k-1})$，进而得到后验分布$p(\bf x_k | \bf x_{k-1},\bf y_k)$，得于是到后验的均值和方差，这就是卡尔曼滤波的想法。

我们先得到$p(\bf x_k | \bf x_{k-1})\sim\mathcal N(\bf\check{x}_k,\bf\check{P}_k)$，即卡尔曼滤波的预测：

$$\begin{aligned} \bf\check{x}_k &= \bf E[\bf x_k] = E[A_k x_{k-1} + w_k + u_k] \\ &= \bf A_kE[x_{k-1}] + w_k + E[u_k] \\ &= \bf A_k\hat{x}_{k-1} + \underbrace {w_k}_\text{输入} \\ \\ \bf\check{P}_k &= \bf E[(x_k - E[x_k])(x_k - E[x_k])^T] \\ &= \bf E[(A_k x_{k-1} + w_k + u_k - A_k\hat{x}_{k-1} - w_k) \\ &\quad \bf *(A_kx_{k-1} + w_k + u_k - A_k\hat{x}_{k-1} - w_k)^T] \\ &= \bf A_k E[(x_{k-1} - \hat{x}_{k-1})(x_{k-1} - \hat{x}_{k-1})^T]A_k^T + E[u_k u_k^T] \\ &= \bf A_k \hat{P}_{k-1} A_k^T + Q_k \end{aligned}$$

对于y：

$$\begin{aligned} \mu_y &= \bf E[\bf y_k] \\ &= \bf C_k\check{x}_k \\ &= \bf C_k(\bf A_k\bf\hat{x}_{k-1} + \bf w_k) \\ \\ \bf\Sigma_{yy} &= \bf E[(y_k - E[y_k])(y_k - E[y_k])^T] \\ &= \bf E[(C_k x_{k} + v_k - C_k\check{x}_k)(C_k x_{k} + v_k - C_k\check{x}_k)^T] \\ &= \bf C_k E[(x_{k} - \check{x}_{k})(x_{k} - \check{x}_{k})^T]C_k^T + E[v_k v_k^T] \\ &= \bf C_k\check{P}_kC_k^T + R_k \end{aligned}$$

要得到联合高斯分布还需要$\bf x,y$的协方差矩阵：

$$\begin{aligned} \bf\Sigma_{xy} &= \bf E[(x_k - E[x_k])(y_k - E[y_k])^T] \\ &= \bf E[(A_k(x_{k-1} - \hat{x}_{k-1}) + u_k)(C_k(A_k(x_{k-1} - \hat{x}_{k-1}) + u_k) + v_k)^T] \\ &= \bf \left( A_k E[(x_{k-1} - \hat{x}_{k-1})(x_{k-1} - \hat{x}_{k-1})^T]A_k^T + E[u_k u_k^T] \right) C_k^T + 0 + 0 + 0 + 0 + 0 \\ &= \bf(\underbrace{A_k \hat{P}_{k-1} A_k^T + Q_k}_{\check{P}_k})C_k^T \\ &= \bf\check{P}_k C_k^T \end{aligned}$$

然后我们直接写出联合高斯分布$\bf p( x_k, y_k | x_{k-1})$：

$$\begin{aligned} \bf p(x_k,y_k | x_{k-1}) &= \mathcal N\left(\begin{bmatrix} \mu_x \\ \mu_y \end{bmatrix}, \begin{bmatrix} \bf\Sigma_{xx}&\bf\Sigma_{xy}\\\bf\Sigma_{yx}&\bf\Sigma_{yy} \end{bmatrix} \right) \\ &= \mathcal N\left(\begin{bmatrix} \bf\check{x}_k \\ \bf C_k\bf\check{x}_k \end{bmatrix}, \begin{bmatrix} \bf\check{P}_k & \bf\check{P}_k C_k^T \\ \bf C_k\check{P}_k & \bf C_k\check{P}_kC_k^T + R_k \end{bmatrix} \right) \end{aligned}$$

由上文展开联合高斯概率密度函数得到后验概率，即卡尔曼滤波器的更新：

$$\begin{aligned} &\bf p(x_k | x_{k-1}, y_k) \\ &= \bf \mathcal N\left( \mu_x + \Sigma_{xy}\Sigma_{yy}^{-1}(y - \mu_y), \quad \Sigma_{xx} - \Sigma_{xy}\Sigma_{yy}^{-1}\Sigma_{yx} \right) \\ &= \bf \mathcal N\left( \overbrace{ \check{x}_k + \underline{\check{P}_k C_k^T(C_k\check{P}_kC_k^T + R_k)^{-1}}(y_k - C_k\check{x}_k)}^{\text{后验均值}\hat x}, \quad \overbrace{(I - \underline{\check{P}_k C_k^T(\bf C_k\check{P}_kC_k^T + R_k)^{-1}} C_k)\check{P}_k}^{\text{后验方差}\hat P} \right) \end{aligned}$$

发现有一些项重复性很高，总结为卡尔曼增益。（注意它的形式跟第三条SMW等式一样，意味着它也可以写成另外的形式）

$$\bf K_k = \bf\check{P}_k C_k^T(C_k\check{P}_kC_k^T + R_k)^{-1}$$

于是后验的均值和方差为:

$$\begin{aligned} \bf\hat{x}_k &= \bf \check{x}_k + K_k(y_k - C_k\check{x}_k) \\ \bf\hat{P}_k &= \bf (I - K_kC_k)\check{P}_k \end{aligned}$$

总结

$$\begin{aligned} 系统建模：& \begin{cases} \bf x_k = A_k x_{k-1} &+ \bf w_k + &\bf u_k \\ \bf y_k = C_k x_{k} &+ &\bf v_k \end{cases} \\\\ 预测(先验)：& \begin{cases} \bf\check{x}_k &= \bf A_k\hat{x}_{k-1} + w_k \\ \bf\check{P}_k &= \bf A_k\hat{P}_{k-1} A_k^T + Q_k \end{cases} \\ 卡尔曼增益：& \begin{cases} \bf K_k &= \bf\check{P}_k C_k^T(C_k\check{P}_kC_k^T + R_k)^{-1} \end{cases} \\ 更新(后验)：& \begin{cases} \bf\hat{x}_k &= \bf \check{x}_k + K_k(y_k - C_k\check{x}_k) \\ \bf\hat{P}_k &= \bf (I - K_kC_k)\check{P}_k \end{cases} \end{aligned}$$