关于Kalman filter的内容已经有很多了，解释的也很清楚。在这里，我们推导一下公式。
在kalman filter中，有两个空间：系统空间和观测空间。
- 系统空间指的是系统内部的各种状态
- 观测空间指的是外部世界中观测到的系统的状态
在系统空间中: $ \hat{x}=Ax_{k-1}+Bu_k $
- $ A $ 从 $ x_{k-1} $ 到 $ x_k $ 的状态转移矩阵
- $ B $ 是控制矩阵
在观测空间: $ z_k=Cx_k+v_k $
- $ C $ 是把系统空间转换到观测空间的观测矩阵
- 观测误差服从协方差矩阵 $ v_k \sim N(0,R) $
- 在时间 $ t-1 $ 的系统误差为 $ P_{k-1} $
估计系统状态:
$ \hat{x}_k=A\hat{x}_k+Bu_k $
时间t的系统误差为 $ P_k=AP_{k-1} A^T $ ，误差推导见文末。
更新系统状态:
观测空间下的最优估计为 $ C\hat{x}_k=C\hat{x}_k + CP_kC^T\left(CP_kC^T+R\right)^{-1}\left(z_k-C\hat{x}_k\right) $
系统空间下的最优估计为 $ \hat{x}_k=\hat{x}_k + P_kC^T\left(CP_kC^T+R\right)^{-1}\left(z_k-C\hat{x}_k\right) $
$ G_k = P_kC^T\left(CP_kC^T+R\right)^{-1} $
在观测空间下最小化系统误差：
$ CP_kC^T=\left(I-CP_kC\left(CP_kC^T+R\right)^{-1}\right)CP_kC^T $
$ P_k=C^{-1}\left(I-CP_kC\left(CP_kC^T+R\right)^{-1}C\right)P_k $
$ P_k=C^{-1}\left(C-CP_kC\left(CP_kC+R\right)^{-1}C\right)P_k $
$ P_k=\left(I-P_kC\left(CP_kC+R\right)^{-1}C\right)P_k=\left(I-G_k\right)P_k $
Kalman filter 五公式:
估计:
$ \hat{x}_k=A\hat{x}_k+Bu_k $
$ P_k=AP_{k-1} A^T $
更新:
$ G_k = P_kC^T\left(CP_kC^T+R\right)^{-1} $
$ \hat{x}_k=\hat{x}_k + G_k\left(z_k-C\hat{x}_k\right) $
$ P_k=\left(I-G_kC\right)P_k $
协方差矩阵计算：
$ cov(X) $
$ =cov([x_1,x_2,x_3]) $
$ =\frac{1}{n-1}\sum_{i=1}^n([x_1,x_2,x_3]^T-[\bar{x}_1,\bar{x}_2,\bar{x}_3]^T)\cdot([x_1,x_2,x_3]-[\bar{x}_1,\bar{x}_2,\bar{x}_3]) $
$ cov(AX) $
$ =cov(A[x_1,x_2,x_3]) $
$ =\frac{1}{n-1}\sum_{i=1}^nA([x_1,x_2,x_3]^T-[\bar{x}_1,\bar{x}_2,\bar{x}_3]^T)\cdot([x_1,x_2,x_3]-[\bar{x}_1,\bar{x}_2,\bar{x}_3])A^T $
$ =Acov(X)A^T $