卡尔曼滤波

基本思想
用前一个状态的估计值和当前观测数据估计当前状态变量的值。

其中：

• 状态变量 $x_k$
• 前一时刻的状态变量$x_{k-1}$
• 第k步状态变量之间增益矩阵的取值$A_k$
• 第k步状态变量和输出信号之间的增益$C_k$
• 量测噪声$v_k$
• 过程噪声$w_{k-1}$
• 当前的观测值$y_k$

状态方程：$x_k=A_{k-1}{x}_{k-}+w_{k-1}$
量测方程：$y_k=C_k{x}_k+v_k$

下一个时刻的观测数据$y_k$由前一个状态变量决定，所以前一个状态变量是否估算准确十分关键。
为了使前一个状态估算准确，使用当前的观测值与实际值的误差去校正前一个状态变量。考虑k-1时刻的过程噪声$w_{k-1}$ 和量测噪声$v_k$。
$$\{\begin{array}{l}{\hat{x}}_k^{\prime }=A_{k-1}{x}_{k-}\\ {\hat{y}}_k^{\prime }=C_k{x}_k\end{array}$$
用数学的语言去描述就是：
$$\begin{array}{rl}{\hat{x}}_k& ={\mathbf{A}}_k{\hat{x}}_{k-1}+{\mathbf{H}}_k(y_k-{\hat{y}}_k^{\prime })\\ & ={\mathbf{A}}_k{\hat{x}}_{k-1}+{\mathbf{H}}_k(y_k-{\mathbf{C}}_k{x}_k)\\ & ={\mathbf{A}}_k{\hat{x}}_{k-1}+{\mathbf{H}}_k(y_k-{\mathbf{C}}_k{\mathbf{A}}_k{\hat{x}}_{k-1})\end{array}$$${\mathbf{H}}_k$为增益矩阵
注意，这里没有考虑k-1时刻的过程噪声$w_{k-1}$ 和量测噪声$v_k$。
这时，经过校正的当前状态的误差为$$\tilde{x}=x_k-{\hat{x}}_k$$
经过校正的当前状态的均方值为$$P_k=E[(x_k-{\hat{x}}_k)(x_k-{\hat{x}}_k{)}^T]$$
未经过校正的当前状态的均方值为$$P_k^{\prime }=E[(x_k-{\hat{x}}_k^{\prime })(x_k-{\hat{x}}_k^{\prime }{)}^T]$$
卡尔曼滤波要求状态变量的估计误差的均方值（ $P_k$对角线元素）为最小， 因此卡尔曼滤波的关键就是要得到Pk与Hk的关系式，即通过选择合适的${\mathbf{H}}_k$,使 $P_k$取得最小值。
经过一系列的推导，得到卡尔曼滤波的推导式：
$$\{\begin{array}{l}{\hat{x}}_k=A_k{\hat{x}}_{k-1}+{\mathbf{H}}_k\left(y_k-{\mathbf{C}}_k{\mathbf{A}}_k{\hat{x}}_{k-1}\right)\\ H_k=P_k^{\prime }{\mathbf{C}}_k^{\mathrm{T}}{\left({\mathbf{C}}_k{P}_k^{\prime }{\mathbf{C}}_k^{\mathrm{T}}+R_k\right)}^{-1}\\ P_k^{\prime }={\mathbf{A}}_k{P}_{k-1}{\mathbf{A}}_k^{\mathrm{T}}+Q_{k-1}\\ P_k=\left(\mathbf{I}-{\mathbf{H}}_k{\mathbf{C}}_k\right)P_k^{\prime }\end{array}$$
其中$Q_k$为过程噪声的方差，$R_k$量测噪声的方差

递推流程：
利用$k-1$时刻的状态变量${\hat{x}}_{k-1}$与经过校正后的状态变量的估计误差均方值$P_k^{\prime }$代入$$P_k^{\prime }={\mathbf{A}}_k{P}_{k-1}{\mathbf{A}}_k^{\mathrm{T}}+Q_{k-1}$$
得到$P_k^{\prime }$, 代入$${\mathbf{H}}_k=P_k^{\prime }{\mathbf{C}}_k^{\mathrm{T}}{\left({\mathbf{C}}_k{P}_k^{\prime }{\mathbf{C}}_k^{\mathrm{T}}+R_k\right)}^{-1}$$
得到${\mathbf{H}}_k$，将其代入
$$P_k=\left(\mathbf{I}-{\mathbf{H}}_k{\mathbf{C}}_k\right)P_k^{\prime }$$
$${\hat{x}}_k=A_k{\hat{x}}_{k-1}+{\mathbf{H}}_k\left(y_k-{\mathbf{C}}_k{\mathbf{A}}_k{\hat{x}}_{k-1}\right)$$
最终得到${\hat{x}}_k$和$P_k$。