卡尔曼滤波

卡尔曼滤波本质上是：非平稳信号下的序贯线性最小均方误差估计量

1、最小均方误差

对于经典的均方误差（Mean Square Error, MSE）方法认为，被估计量 $\boldsymbol{\theta}$ 是一个确定的未知常量，因此经典MSE通过求解下式最小得到估计值
$\text{MSE}(\boldsymbol{\theta}) = \int \| \hat{\boldsymbol{\theta}} - \boldsymbol{\theta} \|^2 p(\boldsymbol{x}) d\boldsymbol{x}$

其中 $\hat{\boldsymbol{\theta}}$ 是观测量 $\boldsymbol{x}$ 的函数，即 $\hat{\boldsymbol{\theta}} = f(\boldsymbol{x})$

而当估计量是一个随机变量时，上述的结果并没有考虑估计量 $\boldsymbol{\theta}$ 的先验 $p(\boldsymbol{\theta})$ ，在添加先验信息后经典MSE将会变成贝叶斯均方误差（Bayes Mean Square Error, BMSE），其表达式如下
$\text{BMSE}(\boldsymbol{\theta}) = \iint \| \hat{\boldsymbol{\theta}} - \boldsymbol{\theta} \|^2 p(\boldsymbol{x}, \boldsymbol{\theta}) d\boldsymbol{x} d\boldsymbol{\theta} =E(\| \hat{\boldsymbol{\theta}} - \boldsymbol{\theta} \|^2)$

同样有 $\hat{\boldsymbol{\theta}} = f(\boldsymbol{x})$ ，其中期望是根据 $p(\boldsymbol{x}, \boldsymbol{\theta})$ 来求解的，使得BMSE最小的 $\hat{\boldsymbol{\theta}}$ 被称为最小贝叶斯均方误差估计量也被称为最小均方误差（Minimum Mean Square Error, MMSE）估计量

2、线性最小均方误差

如果对函数 $\hat{\boldsymbol{\theta}} = f(\boldsymbol{x})$ 做线性假设，即认为估计量由观测 $\boldsymbol{x}$ 线性组合而成，则有 $\hat{\boldsymbol{\theta}} = \boldsymbol{K} \boldsymbol{x} + \boldsymbol{t}$ ，其BMSE可以写成
$\text{BMSE} = E(\| \boldsymbol{K} \boldsymbol{x} + \boldsymbol{t} - \boldsymbol{\theta} \|^2)$

为了使得上式最小，对 $\boldsymbol{t}$ 求导等于 $\boldsymbol{0}$ 可以得到
$\begin{aligned} \frac{\partial }{\partial\boldsymbol{t}}E(\| \boldsymbol{K} \boldsymbol{x} + \boldsymbol{t} - \boldsymbol{\theta} \|^2) =& 0 \\ \Rightarrow E(2\boldsymbol{t} + 2 \boldsymbol{K} \boldsymbol{x}- 2\boldsymbol{\theta}) =& 0 \\ \Rightarrow \boldsymbol{t} =& E(\boldsymbol{\theta}) -\boldsymbol{K} E(\boldsymbol{x}) \end{aligned}$

带入原式中并且继续对 $\boldsymbol{K}$ 进行求导
$\begin{aligned} \frac{\partial }{\partial\boldsymbol{K}}E(\| \boldsymbol{K} (\boldsymbol{x} - E(\boldsymbol{x})) - (\boldsymbol{\theta} - E(\boldsymbol{\theta})) \|^2) = 0 \\ \Rightarrow E(2\boldsymbol{K}(\boldsymbol{x} - E(\boldsymbol{x}))(\boldsymbol{x} - E(\boldsymbol{x}))^T - 2(\boldsymbol{\theta} - E(\boldsymbol{\theta})) (\boldsymbol{x} - E(\boldsymbol{x})^T) = 0 \\ \Rightarrow \boldsymbol{K} = E((\boldsymbol{\theta} - E(\boldsymbol{\theta})) (\boldsymbol{x} - E(\boldsymbol{x})^T)E((\boldsymbol{x} - E(\boldsymbol{x})) (\boldsymbol{x} - E(\boldsymbol{x})^T)^{-1} \\ \Rightarrow \boldsymbol{K} = \boldsymbol{C}_{\boldsymbol{\theta}\boldsymbol{x}}\boldsymbol{C}_{\boldsymbol{x}\boldsymbol{x}}^{-1} \end{aligned}$

最终求得
$\hat{\boldsymbol{\theta}} = E(\boldsymbol{\theta}) + \boldsymbol{C}_{\boldsymbol{\theta}\boldsymbol{x}}\boldsymbol{C}_{\boldsymbol{x}\boldsymbol{x}}^{-1} (\boldsymbol{x} - E(\boldsymbol{x}))$

在 $\boldsymbol{\theta}, \boldsymbol{x}$ 零均值的假设下，即 $E(\boldsymbol{\theta}) = 0, E(\boldsymbol{x}) = 0$ ，上式可以化简成
$\hat{\boldsymbol{\theta}} = \boldsymbol{C}_{\boldsymbol{\theta}\boldsymbol{x}}\boldsymbol{C}_{\boldsymbol{x}\boldsymbol{x}}^{-1} \boldsymbol{x}$

该估计量被称为线性最小方差估计量（Linear Minimum Mean Square Error, LMMSE）

3、卡尔曼滤波

如果所求变量 $\boldsymbol{s}$ 在当前时刻 $n$ 的值 $\boldsymbol{s}[n]$ 和前一时刻的值 $\boldsymbol{s}[n - 1]$ 存在线性关系，即
$\boldsymbol{s}[n] = \boldsymbol{A}[n - 1]\boldsymbol{s}[n - 1]+\boldsymbol{B}[n - 1]\boldsymbol{u}[n]$

其中驱动噪声 $\boldsymbol{u}[n] \backsim \mathcal{N}(\boldsymbol{0},\boldsymbol{Q})$ ，该方程俗称状态方程，表明了变量自身随着时间的变化情况，该方程一般和物理学、运动学等其他学科相关

同时对于所求变量 $\boldsymbol{s}$ 会有相应的观测 $\boldsymbol{x}$ ，在线性假设下，认为有
$\boldsymbol{x}[n] = \boldsymbol{H}[n] \boldsymbol{s}[n] + \boldsymbol{w}[n]$

其中观测噪声 $\boldsymbol{w}[n] \backsim \mathcal{N}(\boldsymbol{0}, \boldsymbol{C}[n])$

令 $\boldsymbol{X}[n] = \begin{bmatrix} \boldsymbol{x}[0] \\ \vdots\\ \boldsymbol{x}[n] \end{bmatrix}$ ，在线性假设下，有 $\hat{\boldsymbol{s}}[n] = \boldsymbol{K} \boldsymbol{X}[n]+\boldsymbol{t}$ ，根据前面LMMSE的结果可知
$\hat{\boldsymbol{s}}[n] = E(\boldsymbol{s}[n]) + \boldsymbol{C}_{\boldsymbol{s}\boldsymbol{X}}\boldsymbol{C}_{\boldsymbol{X}\boldsymbol{X}}^{-1} (\boldsymbol{X}[n] - E(\boldsymbol{X}[n]))$

其中
$\begin{aligned} \boldsymbol{C}_{\boldsymbol{s}\boldsymbol{X}} =& E((\boldsymbol{s}[i] - E(\boldsymbol{s}[i]))(\boldsymbol{X}[n] - E(\boldsymbol{X}[n]))^T) \\ \boldsymbol{C}_{\boldsymbol{X}\boldsymbol{X}} =& E((\boldsymbol{X}[n] - E(\boldsymbol{X}[n]))(\boldsymbol{X}[n] - E(\boldsymbol{X}[n]))^T) \end{aligned}$

上式中 $\hat{\boldsymbol{s}}[n]$ 定义为 $\boldsymbol{X}[n]$ 的线性函数，结果记为 $\hat{\boldsymbol{s}}[n|n]$ ，可以看到每次计算都需要联合 $\boldsymbol{x}[0], \cdots, \boldsymbol{x}[n]$ 的所有数据，考虑到在时序上有很多重复计算，是否可以利用前一帧的结果 $\hat{\boldsymbol{s}}[n-1|n-1]$ 和当前的观测 $\boldsymbol{x}[n]$ 计算得到 $\hat{\boldsymbol{s}}[n|n]$ ，即序贯LMMSE

为了参数分离，首先将 $\boldsymbol{X}[n]$ 分为不相关的 $\boldsymbol{X}[n-1]$ 和新息 $\tilde{\boldsymbol{x}}[n]$ ，这可以通过 Gram-Schmidt 正交化来实现，如下式所示
$\tilde{\boldsymbol{x}}[n] = \boldsymbol{x}[n] - \hat{\boldsymbol{x}}[n|n-1]$

其中 $\hat{\boldsymbol{x}}[n|n-1]$ 是将 $\boldsymbol{x}[n]$ 看做估计量 $\boldsymbol{X}[n-1]$ 看做观测量，在LMMSE下的结果，如下所示
$\hat{\boldsymbol{x}}[n|n-1] = E(\boldsymbol{x}[n]) + \boldsymbol{C}_{\boldsymbol{x}\boldsymbol{X}[n-1]}\boldsymbol{C}_{\boldsymbol{X}[n-1]\boldsymbol{X}[n-1]}^{-1} (\boldsymbol{X}[n-1] - E(\boldsymbol{X}[n-1]))$

可以证明 $E(\tilde{\boldsymbol{x}}[n]\boldsymbol{X}^T[n-1]) = E(\tilde{\boldsymbol{x}}[n])E(\boldsymbol{X}^T[n-1])$ ，过程如下
左边等于
$\begin{aligned} &E(\tilde{\boldsymbol{x}}[n]\boldsymbol{X}^T[n-1]) \\ =& E(\boldsymbol{x}[n]\boldsymbol{X}^T[n-1]) - E(\hat{\boldsymbol{x}}[n|n-1]\boldsymbol{X}^T[n-1]) \\ =& E(\boldsymbol{x}[n]\boldsymbol{X}^T[n-1]) - E(\boldsymbol{x}[n])E(\boldsymbol{X}^T[n-1]) \\ &-\boldsymbol{C}_{\boldsymbol{x}\boldsymbol{X}[n-1]}\boldsymbol{C}_{\boldsymbol{X}[n-1]\boldsymbol{X}[n-1]}^{-1}\boldsymbol{C}_{\boldsymbol{X}[n-1]\boldsymbol{X}[n-1]} \\ =& E(\boldsymbol{x}[n]\boldsymbol{X}^T[n-1]) - E(\boldsymbol{x}[n])E(\boldsymbol{X}^T[n-1]) - \boldsymbol{C}_{\boldsymbol{x}\boldsymbol{X}[n-1]} \\ =& \boldsymbol{C}_{\boldsymbol{x}\boldsymbol{X}[n-1]} - \boldsymbol{C}_{\boldsymbol{x}\boldsymbol{X}[n-1]} \\ =& 0 \end{aligned}$

右边等于
$E(\tilde{\boldsymbol{x}}[n]) = E(\boldsymbol{x}[n]) - E(\hat{\boldsymbol{x}}[n|n-1]) = 0$

将 $\boldsymbol{X}[n-1], \tilde{\boldsymbol{x}}[n]$ 组成新的向量 $\boldsymbol{X}'[n] = \begin{bmatrix} \boldsymbol{X}[n-1]\\ \tilde{\boldsymbol{x}}[n] \end{bmatrix}$ ，注意到 $\boldsymbol{X}[n]$ 可以由 $\boldsymbol{X}'[n]$ 线性变换得到，所以两者的 LMMSE 结果一致，将前面得到的关于 $\boldsymbol{X}[n]$ 的 LMMSE 结果转变成关于 $\boldsymbol{X}'[n]$ 的 LMMSE 结果，如下所示
$\hat{\boldsymbol{s}}[n|n] = E(\boldsymbol{s}[n]) + \boldsymbol{C}_{\boldsymbol{s}\boldsymbol{X}'}\boldsymbol{C}_{\boldsymbol{X}'\boldsymbol{X}'}^{-1} (\boldsymbol{X}'[n] - E(\boldsymbol{X}'[n]))$

其中
$\begin{aligned} \boldsymbol{C}_{\boldsymbol{s}\boldsymbol{X}'} = \begin{bmatrix} \boldsymbol{C}_{\boldsymbol{s}\boldsymbol{X}[n-1]} & \boldsymbol{C}_{\boldsymbol{s}\tilde{\boldsymbol{x}}[n]} \end{bmatrix} \\ \boldsymbol{C}_{\boldsymbol{X}'\boldsymbol{X}'} = \begin{bmatrix} \boldsymbol{C}_{\boldsymbol{X}[n-1]\boldsymbol{X}[n-1]} & \boldsymbol{0} \\ \boldsymbol{0} &\boldsymbol{C}_{\tilde{\boldsymbol{x}}[n]\tilde{\boldsymbol{x}}[n]} \end{bmatrix} \end{aligned}$

注意到矩阵中存在部分0，因此转变后的 LMMSE 结果可以拆成
$\begin{aligned} \hat{\boldsymbol{s}}[n|n] =& E(\boldsymbol{s}[n]) + \boldsymbol{C}_{\boldsymbol{s}[n]\boldsymbol{X}[n-1]}\boldsymbol{C}_{\boldsymbol{X}[n-1]\boldsymbol{X}[n-1]}^{-1} (\boldsymbol{X}[n-1] - E(\boldsymbol{X}[n-1]))\\ & + \boldsymbol{C}_{\boldsymbol{s}\tilde{\boldsymbol{x}}[n]}\boldsymbol{C}_{\tilde{\boldsymbol{x}}[n]\tilde{\boldsymbol{x}}[n]}^{-1} (\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n])) \end{aligned}$

利用 $\boldsymbol{s}[n] - E(\boldsymbol{s}[n]) = \boldsymbol{A}[n - 1]\boldsymbol{s}[n - 1] - \boldsymbol{A}[n - 1]E(\boldsymbol{s}[n - 1])+\boldsymbol{B}[n - 1]\boldsymbol{u}[n]$ 可以得到
$\begin{aligned} \boldsymbol{C}_{\boldsymbol{s}[n]\boldsymbol{X}[n-1]} =& E((\boldsymbol{s}[n] - E(\boldsymbol{s}[n]))(\boldsymbol{X}[n - 1] - E(\boldsymbol{X}[n - 1]))^T)\\ =& \boldsymbol{A}[n - 1]\boldsymbol{C}_{\boldsymbol{s}[n-1]\boldsymbol{X}[n-1]} \end{aligned}$

继而可以得到
$\begin{aligned} &\hat{\boldsymbol{s}}[n|n]\\ =& \boldsymbol{A}[n - 1]E(\boldsymbol{s}[n - 1]) \\ &+ \boldsymbol{A}[n - 1]\boldsymbol{C}_{\boldsymbol{s}[n-1]\boldsymbol{X}[n-1]}\boldsymbol{C}_{\boldsymbol{X}[n-1]\boldsymbol{X}[n-1]}^{-1} (\boldsymbol{X}[n-1] - E(\boldsymbol{X}[n-1]))\\ & + \boldsymbol{C}_{\boldsymbol{s}\tilde{\boldsymbol{x}}[n]}\boldsymbol{C}_{\tilde{\boldsymbol{x}}[n]\tilde{\boldsymbol{x}}[n]}^{-1} (\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n])) \\ =& \boldsymbol{A}[n - 1] \hat{\boldsymbol{s}}[n-1|n-1] + \boldsymbol{C}_{\boldsymbol{s}\tilde{\boldsymbol{x}}[n]}\boldsymbol{C}_{\tilde{\boldsymbol{x}}[n]\tilde{\boldsymbol{x}}[n]}^{-1} (\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n])) \end{aligned}$

如果令 $\hat{\boldsymbol{s}}[n|n-1] = \boldsymbol{A}[n - 1] \hat{\boldsymbol{s}}[n-1|n-1]$ ，则有
$\begin{aligned} \hat{\boldsymbol{s}}[n|n] =& \hat{\boldsymbol{s}}[n|n-1] + \boldsymbol{C}_{\boldsymbol{s}\tilde{\boldsymbol{x}}[n]}\boldsymbol{C}_{\tilde{\boldsymbol{x}}[n]\tilde{\boldsymbol{x}}[n]}^{-1} (\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n])) \end{aligned}$

其中
$\begin{aligned} \boldsymbol{C}_{\boldsymbol{s}\tilde{\boldsymbol{x}}[n]} =& E((\boldsymbol{s}[n] - E(\boldsymbol{s}[n]))(\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))^T) \\ \boldsymbol{C}_{\tilde{\boldsymbol{x}}[n]\tilde{\boldsymbol{x}}[n]} =& E((\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))(\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))^T) \end{aligned}$

根据 $\hat{\boldsymbol{x}}[n|n - 1]$ 的表达式，很容易证明
$\hat{\boldsymbol{x}}[n|n - 1] = \boldsymbol{H}[n]\hat{\boldsymbol{s}}[n|n - 1]$

因此有
$\begin{aligned} \tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]) =& \tilde{\boldsymbol{x}}[n] \\ =& \boldsymbol{x}[n] - \hat{\boldsymbol{x}}[n|n-1] \\ =& \boldsymbol{x}[n] - \boldsymbol{H}[n]\hat{\boldsymbol{s}}[n|n - 1] \\ =& \boldsymbol{H}[n]\boldsymbol{s}[n] + \boldsymbol{w}[n] - \boldsymbol{H}[n]\hat{\boldsymbol{s}}[n|n - 1] \\ =& \boldsymbol{H}[n](\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1]) + \boldsymbol{w}[n] \end{aligned}$

利用 $\hat{\boldsymbol{s}}[n|n-1] = E(\boldsymbol{s}[n]) + \boldsymbol{C}_{\boldsymbol{s}\boldsymbol{X}[n-1]}\boldsymbol{C}_{\boldsymbol{X}[n-1]\boldsymbol{X}[n-1]}^{-1} (\boldsymbol{X}[n-1] - E(\boldsymbol{X}[n-1]))$ 和 $E((\boldsymbol{X}[n-1] - E(\boldsymbol{X}[n-1]))(\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))^T) = 0$ ，可以得到
$\begin{aligned} &\boldsymbol{C}_{\boldsymbol{s}\tilde{\boldsymbol{x}}[n]}\\ =& E((\boldsymbol{s}[n] - E(\boldsymbol{s}[n]))(\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))^T) \\ =& E((\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])(\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))^T) \\ =& E((\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])(\boldsymbol{H}[n](\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1]) + \boldsymbol{w}[n])^T) \\ =& E((\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])(\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])^T) \boldsymbol{H}^T[n] \end{aligned}$

令 $\boldsymbol{M}[n|n - 1] = E((\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])(\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])^T)$ ，则上式变成
$\boldsymbol{C}_{\boldsymbol{s}\tilde{\boldsymbol{x}}[n]} = \boldsymbol{M}[n|n - 1]\boldsymbol{H}^T[n]$

另一方面利用 $\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]) = \boldsymbol{H}[n](\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1]) + \boldsymbol{w}[n]$ ，可以得到
$\begin{aligned} \boldsymbol{C}_{\tilde{\boldsymbol{x}}[n]\tilde{\boldsymbol{x}}[n]} =& E((\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))(\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))^T) \\ =& \boldsymbol{H}[n] \boldsymbol{M}[n|n - 1]\boldsymbol{H}^T[n] + \boldsymbol{C}[n] \end{aligned}$

因此有
$\hat{\boldsymbol{s}}[n|n] = \hat{\boldsymbol{s}}[n|n-1] + \boldsymbol{K}[n](\boldsymbol{x}[n] - \boldsymbol{H}[n]\hat{\boldsymbol{s}}[n|n - 1])$

其中
$\begin{aligned} \boldsymbol{K}[n] =& \boldsymbol{C}_{\boldsymbol{s}\boldsymbol{X}[n-1]}\boldsymbol{C}_{\boldsymbol{X}[n-1]\boldsymbol{X}[n-1]}^{-1} \\ =& \boldsymbol{M}[n|n - 1]\boldsymbol{H}^T[n](\boldsymbol{H}[n] \boldsymbol{M}[n|n - 1]\boldsymbol{H}^T[n] + \boldsymbol{C}[n])^{-1} \end{aligned}$

最后需要给出 $\boldsymbol{M}[n|n - 1]$ 和 $\boldsymbol{M}[n|n]$ 的递推公式
利用 $\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1] = \boldsymbol{A}[n - 1]\boldsymbol{s}[n-1] + \boldsymbol{B}[n - 1]\boldsymbol{u[n]} - \boldsymbol{A}[n - 1] \hat{\boldsymbol{s}}[n - 1|n - 1]$ ，可以得到
$\begin{aligned} \boldsymbol{M}[n|n - 1] =& E((\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])(\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])^T) \\ =& \boldsymbol{A}[n - 1]\boldsymbol{M}[n-1|n-1]\boldsymbol{A}^T[n - 1] + \boldsymbol{B}[n - 1]\boldsymbol{Q}\boldsymbol{B}^T[n - 1] \end{aligned}$

利用 $\hat{\boldsymbol{s}}[n|n] = \hat{\boldsymbol{s}}[n|n-1] + \boldsymbol{K}[n](\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))$ 可以得到
$\begin{aligned} \boldsymbol{M}[n|n] =& E((\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n])(\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n])^T) \\ =& E((\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])(\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])^T)\\ &- 2E((\boldsymbol{s}[n] - \hat{\boldsymbol{s}}[n|n - 1])(\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))^T) \boldsymbol{K}^T[n]\\ &+\boldsymbol{K}[n] E(\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))(\tilde{\boldsymbol{x}}[n] - E(\tilde{\boldsymbol{x}}[n]))^T) \boldsymbol{K}^T[n]\\ =& \boldsymbol{M}[n|n-1] \\ &- 2 \boldsymbol{M}[n|n-1] \boldsymbol{H}^T[n]\boldsymbol{K}^T[n]\\ &+ \boldsymbol{M}[n|n-1] \boldsymbol{H}^T[n]\boldsymbol{K}^T[n] \\ =& (\boldsymbol{I} - \boldsymbol{K}[n]\boldsymbol{H}[n])\boldsymbol{M}[n|n - 1] \end{aligned}$

至此所有递推公式都推导完毕，可以很清楚地看到一切皆起源于方程
$\hat{\boldsymbol{s}}[n] = E(\boldsymbol{s}[n]) + \boldsymbol{C}_{\boldsymbol{s}\boldsymbol{X}}\boldsymbol{C}_{\boldsymbol{X}\boldsymbol{X}}^{-1} (\boldsymbol{X}[n] - E(\boldsymbol{X}[n]))$

后面的推导只是在完成该方程序贯形式下的结果罢了，而该方程的起源来自于线性假设，所以卡尔曼滤波本质上是非平稳信号下的序贯线性最小均方误差估计量，其最终结果可以总结如下步骤：
初始化：
$\hat{\boldsymbol{s}}[-1|-1] = \boldsymbol{\mu}_{\boldsymbol{s}}, \boldsymbol{M}[-1|-1] = \boldsymbol{C}_{\boldsymbol{s}}$

预测估计量：
$\hat{\boldsymbol{s}}[n|n - 1] = \boldsymbol{A}[n - 1]\hat{\boldsymbol{s}}[n - 1|n - 1]$

预测最小 MSE 矩阵：
$\boldsymbol{M}[n|n - 1] = \boldsymbol{A}[n - 1] \boldsymbol{M}[n - 1| n - 1] \boldsymbol{A}^T[n - 1] + \boldsymbol{B}[n - 1]\boldsymbol{Q}\boldsymbol{B}^T[n - 1]$

增益矩阵：
$\boldsymbol{K}[n] = \boldsymbol{M}[n|n - 1] \boldsymbol{H}^T[n](\boldsymbol{H}[n]\boldsymbol{M}[n|n - 1]\boldsymbol{H}^T[n] + \boldsymbol{C}[n])^{-1}$

修正估计量：
$\hat{\boldsymbol{s}}[n|n] = \hat{\boldsymbol{s}}[n|n - 1] + \boldsymbol{K}[n](\boldsymbol{x}[n] - \boldsymbol{H}[n] \hat{\boldsymbol{s}}[n|n - 1])$

修正最小 MSE 矩阵：
$\boldsymbol{M}[n|n] = (\boldsymbol{I} - \boldsymbol{K}[n] \boldsymbol{H}[n]) \boldsymbol{M}[n|n - 1]$

4、扩展卡尔曼滤波

卡尔曼滤波基于线性假设，当状态方程和观测方程均不为线性的时候，如下所示
$\begin{aligned} \boldsymbol{s}[n] =& a(\boldsymbol{s}[n - 1]) + \boldsymbol{B}[n - 1]\boldsymbol{u}[n] \\ \boldsymbol{x}[n] =& h(\boldsymbol{s}[n]) + \boldsymbol{w}[n] \end{aligned}$

如果认为前一时刻的值几乎在真值附近，将非线性函数 $a,h$ 进行泰勒展开可以得到
$\begin{aligned} a(\boldsymbol{s}[n - 1]) \approx& a(\tilde{\boldsymbol{s}}[n - 1 | n - 1]) + \boldsymbol{A}[n - 1](\boldsymbol{s}[n - 1] - \tilde{\boldsymbol{s}}[n - 1 | n - 1]) \\ h(\boldsymbol{s}[n]) \approx& h(\tilde{\boldsymbol{s}}[n| n - 1]) + \boldsymbol{H}[n](\boldsymbol{s}[n] - \tilde{\boldsymbol{s}}[n | n - 1]) \\ \end{aligned}$

其中 $\boldsymbol{A}[n - 1] = \frac{\partial a(\boldsymbol{x})}{\partial \boldsymbol{x}}\bigg|_{\boldsymbol{x} = \tilde{\boldsymbol{s}}[n - 1 | n - 1]}, \boldsymbol{H}[n] = \frac{\partial h(\boldsymbol{x})}{\partial \boldsymbol{x}} \bigg|_{\boldsymbol{x} = \tilde{\boldsymbol{s}}[n | n - 1]}$

则原状态和观测方程将会变为
$\begin{aligned} \boldsymbol{s}[n] =& \boldsymbol{A}[n - 1] \boldsymbol{s}[n - 1] + \boldsymbol{B}[n - 1]\boldsymbol{u}[n] + \boldsymbol{a}[n - 1] \\ \boldsymbol{x}[n] =& \boldsymbol{H}[n] \boldsymbol{s}[n] + \boldsymbol{w}[n] + \boldsymbol{h}[n - 1] \end{aligned}$

其中 $\boldsymbol{a}[n - 1] = a(\tilde{\boldsymbol{s}}[n - 1 | n - 1]) - \boldsymbol{A}[n - 1] \tilde{\boldsymbol{s}}[n - 1 | n - 1]$ ， $\boldsymbol{h}[n - 1] = h(\tilde{\boldsymbol{s}}[n| n - 1]) - \boldsymbol{H}[n] \tilde{\boldsymbol{s}}[n | n - 1]$

最终结果可以总结如下步骤：
初始化：
$\hat{\boldsymbol{s}}[-1|-1] = \boldsymbol{\mu}_{\boldsymbol{s}}, \boldsymbol{M}[-1|-1] = \boldsymbol{C}_{\boldsymbol{s}}$

预测估计量：
$\hat{\boldsymbol{s}}[n|n - 1] = a(\hat{\boldsymbol{s}}[n - 1|n - 1])$

预测最小 MSE 矩阵：
$\boldsymbol{M}[n|n - 1] = \boldsymbol{A}[n - 1] \boldsymbol{M}[n - 1| n - 1] \boldsymbol{A}^T[n - 1] + \boldsymbol{B}[n - 1]\boldsymbol{Q}\boldsymbol{B}^T[n - 1]$

增益矩阵：
$\boldsymbol{K}[n] = \boldsymbol{M}[n|n - 1] \boldsymbol{H}^T[n](\boldsymbol{H}[n]\boldsymbol{M}[n|n - 1]\boldsymbol{H}^T[n] + \boldsymbol{C}[n])^{-1}$

修正估计量：
$\hat{\boldsymbol{s}}[n|n] = \hat{\boldsymbol{s}}[n|n - 1] + \boldsymbol{K}[n](\boldsymbol{x}[n] - h(\hat{\boldsymbol{s}}[n|n - 1]))$

修正最小 MSE 矩阵：
$\boldsymbol{M}[n|n] = (\boldsymbol{I} - \boldsymbol{K}[n] \boldsymbol{H}[n]) \boldsymbol{M}[n|n - 1]$

可以看到与前面有差别的部分主要在于 “预测估计量” 和 “修正估计量” 两个步骤

最后编辑于：2023.06.05 09:56:50

人面猴
序言：七十年代末，一起剥皮案震惊了整个滨河市，随后出现的几起案子，更是在滨河造成了极大的恐慌，老刑警刘岩，带你破解...
沈念sama阅读 220,295评论 6赞 512
死咒
序言：滨河连续发生了三起死亡事件，死亡现场离奇诡异，居然都是意外死亡，警方通过查阅死者的电脑和手机，发现死者居然都...
沈念sama阅读 93,928评论 3赞 396
救了他两次的神仙让他今天三更去死
文/潘晓璐我一进店门，熙熙楼的掌柜王于贵愁眉苦脸地迎上来，“玉大人，你说我怎么就摊上这事。” “怎么了？”我有些...
开封第一讲书人阅读 166,682评论 0赞 357
道士缉凶录：失踪的卖姜人
文/不坏的土叔我叫张陵，是天一观的道长。经常有香客问我，道长，这世上最难降的妖魔是什么？我笑而不...
开封第一讲书人阅读 59,209评论 1赞 295
港岛之恋（遗憾婚礼）
正文为了忘掉前任，我火速办了婚礼，结果婚礼上，老公的妹妹穿的比我还像新娘。我一直安慰自己，他们只是感情好，可当我...
茶点故事阅读 68,237评论 6赞 397
恶毒庶女顶嫁案：这布局不是一般人想出来的
文/花漫我一把揭开白布。她就那样静静地躺着，像睡着了一般。火红的嫁衣衬着肌肤如雪。梳的纹丝不乱的头发上，一...
开封第一讲书人阅读 51,965评论 1赞 308
城市分裂传说
那天，我揣着相机与录音，去河边找鬼。笑死，一个胖子当着我的面吹牛，可吹牛的内容都是我干的。我是一名探鬼主播，决...
沈念sama阅读 40,586评论 3赞 420
双鸳鸯连环套：你想象不到人心有多黑
文/苍兰香墨我猛地睁开眼，长吁一口气：“原来是场噩梦啊……” “哼！你这毒妇竟也来了？” 一声冷哼从身侧响起，我...
开封第一讲书人阅读 39,487评论 0赞 276
万荣杀人案实录
序言：老挝万荣一对情侣失踪，失踪者是张志新（化名）和其女友刘颖，没想到半个月后，有当地人在树林里发现了一具尸体，经...
沈念sama阅读 46,016评论 1赞 319
护林员之死
正文独居荒郊野岭守林人离奇死亡，尸身上长有42处带血的脓包…… 初始之章·张勋以下内容为张勋视角年9月15日...
茶点故事阅读 38,136评论 3赞 340
白月光启示录
正文我和宋清朗相恋三年，在试婚纱的时候发现自己被绿了。大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
茶点故事阅读 40,271评论 1赞 352
活死人
序言：一个原本活蹦乱跳的男人离奇死亡，死状恐怖，灵堂内的尸体忽然破棺而出，到底是诈尸还是另有隐情，我是刑警宁泽，带...
沈念sama阅读 35,948评论 5赞 347
日本核电站爆炸内幕
正文年R本政府宣布，位于F岛的核电站，受9级特大地震影响，放射性物质发生泄漏。R本人自食恶果不足惜，却给世界环境...
茶点故事阅读 41,619评论 3赞 331
男人毒药：我在死后第九天来索命
文/蒙蒙一、第九天我趴在偏房一处隐蔽的房顶上张望。院中可真热闹，春花似锦、人声如沸。这庄子的主人今日做“春日...
开封第一讲书人阅读 32,139评论 0赞 23
一桩弑父案，背后竟有这般阴谋
文/苍兰香墨我抬头看了看天上的太阳。三九已至，却和暖如春，着一层夹袄步出监牢的瞬间，已是汗流浃背。一阵脚步声响...
开封第一讲书人阅读 33,252评论 1赞 272
情欲美人皮
我被黑心中介骗来泰国打工，没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留，地道东北人。一个月前我还...
沈念sama阅读 48,598评论 3赞 375
代替公主和亲
正文我出身青楼，却偏偏与公主长得像，于是被迫代替她去往敌国和亲。传闻我的和亲对象是个残疾皇子，可洞房花烛夜当晚...
茶点故事阅读 45,267评论 2赞 358

卡尔曼滤波

1、最小均方误差

2、线性最小均方误差

3、卡尔曼滤波

4、扩展卡尔曼滤波

推荐阅读更多精彩内容