离散时间下的卡尔曼滤波器

  已知运动方程为，
$$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$$