1 状态估计问题表述

运动方程：
$${\mathbf{x}}_k=\mathbf{f}({\mathbf{x}}_{k-1},{\mathbf{u}}_k)+{\mathbf{w}}_k\text{\hspace{1em}(1)}$$
其中$\mathbf{f}$为运动方程，${\mathbf{w}}_k\sim \mathcal{N}(0,R_k)$。

测量方程：
$${\mathbf{z}}_k=\mathbf{h}({\mathbf{x}}_k)+{\mathbf{v}}_k\text{\hspace{1em}(2)}$$

2 卡尔曼滤波器

前提条件：$\mathbf{f}$和$\mathbf{h}$是线性函数。

那么有，
$$\begin{array}{rl}{\mathbf{x}}_k& ={\mathbf{A}}_k{\mathbf{x}}_{k-1}+{\mathbf{u}}_k+{\mathbf{w}}_k\\ {\mathbf{z}}_k& ={\mathbf{C}}_k{\mathbf{x}}_k+{\mathbf{v}}_k\end{array}\text{\hspace{1em}(3)}$$

预测：
$$\begin{array}{rl}{\mathbf{x}}_{k,pred}& ={\mathbf{A}}_k{\mathbf{x}}_{k-1}+{\mathbf{u}}_k\\ {\mathbf{P}}_{k,pred}& ={\mathbf{A}}_k{\mathbf{P}}_{k-1}{\mathbf{A}}_k^T+{\mathbf{R}}_k\end{array}\text{\hspace{1em}(4)}$$

计算卡尔曼增益$\mathbf{K}$：

$${\mathbf{K}}_k={\mathbf{P}}_{k,pred}{\mathbf{C}}_k^T({\mathbf{C}}_k{\mathbf{P}}_{k,pred}{\mathbf{C}}_k^T+{\mathbf{Q}}_k{)}^{-1}\text{\hspace{1em}(5)}$$

更新：
$$\begin{array}{rl}{\mathbf{x}}_k& ={\mathbf{x}}_{k,pred}+{\mathbf{K}}_k({\mathbf{z}}_k-{\mathbf{C}}_k{\mathbf{x}}_{k,pred})\\ {\mathbf{P}}_k& =(\mathbf{I}-{\mathbf{K}}_k{\mathbf{C}}_k){\mathbf{P}}_{k,pred}\end{array}\text{\hspace{1em}(6)}$$

3 扩展卡尔曼滤波器

前提条件：$\mathbf{f}$和$\mathbf{h}$是非线性函数。

线性化之后的运行方程和测量方程如下，
$$\begin{array}{rl}{\mathbf{x}}_k& \approx \mathbf{f}({\mathbf{x}}_{k-1},{\mathbf{u}}_k)+{\mathbf{F}}_k\Delta {\mathbf{x}}_k+{\mathbf{w}}_k\\ {\mathbf{z}}_k& \approx \mathbf{h}({\mathbf{x}}_{k,pred})+{\mathbf{H}}_k({\mathbf{x}}_k-{\mathbf{x}}_{k,pred})+{\mathbf{n}}_k\end{array}\text{\hspace{1em}(7)}$$

预测：

$$\begin{array}{rl}{\mathbf{x}}_{k,pred}& =\mathbf{f}({\mathbf{x}}_{k-1},{\mathbf{u}}_k)\\ {\mathbf{P}}_{k,pred}& ={\mathbf{F}}_k{\mathbf{P}}_{k-1}{\mathbf{F}}_k^T+{\mathbf{R}}_k\end{array}\text{\hspace{1em}(8)}$$

计算扩展卡尔曼增益${\mathbf{K}}_k$，
$${\mathbf{K}}_k={\mathbf{P}}_{k,pred}{\mathbf{H}}_k^T({\mathbf{H}}_k{\mathbf{P}}_{k,pred}{\mathbf{H}}_k^T+{\mathbf{Q}}_k{)}^{-1}\text{\hspace{1em}(9)}$$

更新：

$$\begin{array}{rl}{\mathbf{x}}_k& ={\mathbf{x}}_{k,pred}+{\mathbf{K}}_k({\mathbf{z}}_k-\mathbf{h}({\mathbf{x}}_{k,pred}))\\ {\mathbf{P}}_k& =(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k){\mathbf{P}}_{k,pred}\end{array}\text{\hspace{1em}(10)}$$

4 其他

已知运动方程为，
$$x_k={\varphi }_{k|k-1}{x}_{k-1}+{\Gamma }_{k-1}{w}_{k-1}\text{\hspace{1em}(1)}$$
其中$x_k$表示$k$时刻的状态，${\varphi }_{k|k-1}$表示状态转移矩阵，$x_{k-1}$表示$k-1$时刻的状态，${\Gamma }_{k-1}$表示噪声矩阵，$w_{k-1}$表示运动噪声。$w_{k-1}$服从高斯分布$N(0,Q_{k-1})$。
  测量方程为，
$$z_k=H_k{x}_k+v_k\text{\hspace{1em}(2)}$$
其中$z_k$表示$k$时刻的测量，$H_k$表示测量矩阵，$v_k$表示测量噪声。$v_k$服从高斯分布$N(0,R_k)$。
  输入$k-1$时刻的后验状态${\hat{x}}_{k-1}$和后验状态的协方差矩阵${\hat{P}}_{k-1}$，以及$k$时刻的测量$z_k$，进行如下操作：预测+更新。
  预测：用运动方程得到先验估计，包括先验状态${\check{x}}_k$和先验状态的协方差矩阵${\check{P}}_k$。
$${\check{x}}_k={\varphi }_{k|k-1}{\hat{x}}_{k-1}\text{\hspace{1em}(3-1)}$$
$${\check{P}}_k={\varphi }_{k|k-1}{\hat{P}}_{k-1}{\varphi }_{k|k-1}^T+{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{k-1}^T\text{\hspace{1em}(3-2)}$$
其中公式(3-1)由均值的传播定律得到，而公式(3-2)由协方差的传播定律得到。
  更新：先计算卡尔曼增益，然后用测量方程更新先验估计得到后验估计，包括后验状态${\hat{x}}_k$和后验状态的协方差矩阵${\hat{P}}_k$。
$$K_k={\check{P}}_k{H}_k^T(H_k{\check{P}}_k{H}_k^T+R_k{)}^{-1}\text{\hspace{1em}(3-3)}$$

$${\hat{x}}_k={\check{x}}_k+K_k(z_k-H_k{\check{x}}_k)\text{\hspace{1em}(3-4)}$$

$${\hat{P}}_k=(I-K_k{H}_k){\check{P}}_k\text{\hspace{1em}(3-5)}$$
其中公式(3-1)至(3-5)即为离散时间下的卡尔曼滤波器方程。

  卡尔曼滤波器的一个小例子。
  已知房间内的温度受扰动噪声$w$的影响，它服从高斯分布$N(0,0.4^2)$；同时，房间内有一个温度计，它的测量噪声为$v$，它服从高斯分布$N(0,0.3^2)$。已知$k-1$时刻房间内的真实温度为25摄氏度，$k$时刻的温度计读数为25.2摄氏度，请问$k$时刻房间的温度估计为多少？该估计的不确定度为多少？
  解答：
由题意，系统的运动方程为，
$$x_k=x_{k-1}+w_{k-1}$$
测量方程为，
$$z_k=x_k+v_k$$
且已知$k-1$时刻的后验估计为25摄氏度，方差为0。根据公式(3-1)和(3-2)得到，$k$时刻先验的状态估计为25摄氏度，方差为$0.4^2$。根据公式(3-3)得到卡尔曼增益为，
$$K_k=\frac{0.4^2}{0.4^2+0.3^2}=\frac{16}{25}=0.64$$
那么，根据公式(3-4)和公式(3-5)得到后验的状态估计为，
$${\hat{x}}_k=25+0.64\cdot (25.2-25)=25.128$$
$${\hat{P}}_k=(1-0.64)\cdot 0.4^2=0.0576=0.24^2$$
故$k$时刻房间的温度估计为25.128摄氏度，该估计的不确定度为$0.24^2$。
  换个角度思考（当运动方程和测量方程中的系数皆为1时，下面两个结论才成立），
预测操作时，状态的不确定度等价于电阻的串联，
$${\check{P}}_k=0^2+0.4^2\Rightarrow {\check{P}}_k=0.4^2$$
更新操作时，状态的不确定度等价于电阻的并联，
$$\frac{1}{{\hat{P}}_k}=\frac{1}{0.4^2}+\frac{1}{0.3^2}\Rightarrow {\hat{P}}_k=0.24^2$$