【卡尔曼滤波理论推导与实践】【理论】【4/5 公式总结】

来看一下推导的思维流程：

1. 卡尔曼滤波核心思想是推导的源头，也是推导的终点：
   $$\begin{array}{rl}\overrightarrow{z_{k估}}& =\overrightarrow{z_{k模}}+{\mathrm{G}}_{\mathrm{k}}\ast ({\mathrm{H}}^{-1}\ast \overrightarrow{y_{k测}}-\overrightarrow{z_{k模}})\end{array}$$
2. $\overrightarrow{z_{k模}}=\mathrm{A}\ast \overrightarrow{z_{k-1估}}+\mathrm{B}\ast F_{k-1}$来自状态转移方程，$\overrightarrow{y_{k测}}$是测量得到的。
3. 推导出${\mathrm{G}}_{\mathrm{k}}=\frac{{\mathrm{P}}_{\mathrm{k}\mathrm{模}}}{{\mathrm{P}}_{\mathrm{k}\mathrm{模}}+({\mathrm{H}}^{-1}{)}^2\mathrm{R}}$
4. 推导出${\mathrm{P}}_{\mathrm{k}\mathrm{模}}=\mathrm{A}{\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}{\mathrm{A}}^T+\mathrm{Q}$
5. 推导出${\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}=(\mathrm{I}-{\mathrm{G}}_{\mathrm{k}-1}){\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}$

观察上述5个公式，可以发现是一个迭代计算的过程，按照顺序一个个看下来，可以发现等式右侧全是已知数。把与模型相关的公式放在前面，迭代计算过程为：

1. $\overrightarrow{z_{k模}}=\mathrm{A}\ast \overrightarrow{z_{k-1估}}+\mathrm{B}\ast F_{k-1}$
2. ${\mathrm{P}}_{\mathrm{k}\mathrm{模}}=\mathrm{A}{\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}{\mathrm{A}}^T+\mathrm{Q}$
3. ${\mathrm{G}}_{\mathrm{k}}=\frac{{\mathrm{P}}_{\mathrm{k}\mathrm{模}}}{{\mathrm{P}}_{\mathrm{k}\mathrm{模}}+({\mathrm{H}}^{-1}{)}^2\mathrm{R}}$
4. $\overrightarrow{z_{k估}}=\overrightarrow{z_{k模}}+{\mathrm{G}}_{\mathrm{k}}\ast ({\mathrm{H}}^{-1}\ast \overrightarrow{y_{k测}}-\overrightarrow{z_{k模}})$
5. ${\mathrm{P}}_{\mathrm{k}\mathrm{估}}=(\mathrm{I}-{\mathrm{G}}_{\mathrm{k}}){\mathrm{P}}_{\mathrm{k}\mathrm{模}}$

卡尔曼的推导过程中将卡尔曼增益做了一个变换${\mathrm{G}}_{\mathrm{k}}={\mathrm{K}}_{\mathrm{k}}\mathrm{H}$，${\mathrm{K}}_{\mathrm{k}}$是一个对角矩阵。结果是一样的，注意对角矩阵$\mathrm{H}$左乘、右乘、转置是一样的，${\mathrm{K}}_{\mathrm{k}}$形式的5个公式是：

1. $\overrightarrow{z_{k模}}=\mathrm{A}\ast \overrightarrow{z_{k-1估}}+\mathrm{B}\ast F_{k-1}$
2. ${\mathrm{P}}_{\mathrm{k}\mathrm{模}}=\mathrm{A}{\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}{\mathrm{A}}^T+\mathrm{Q}$
3. ${\mathrm{K}}_{\mathrm{k}}=\frac{{\mathrm{P}}_{\mathrm{k}\mathrm{模}}{\mathrm{H}}^T}{\mathrm{H}{\mathrm{P}}_{\mathrm{k}\mathrm{模}}{\mathrm{H}}^T+\mathrm{R}}$
4. $\overrightarrow{z_{k估}}=\overrightarrow{z_{k模}}+{\mathrm{K}}_{\mathrm{k}}\ast (\overrightarrow{y_{k测}}-\mathrm{H}\ast \overrightarrow{z_{k模}})$
5. ${\mathrm{P}}_{\mathrm{k}\mathrm{估}}=(\mathrm{I}-{\mathrm{K}}_{\mathrm{k}}\mathrm{H}){\mathrm{P}}_{\mathrm{k}\mathrm{模}}$

$\mathrm{A}$是状态转移矩阵，是人为建模给定的。
$\mathrm{B}$是控制矩阵，是人为建模给定的。
$\mathrm{Q},\mathrm{R}$分别是正态分布模型噪声和测量噪声的协方差矩阵，是人为假设的，这也是需要调参的点。
$\overrightarrow{z_{k估}}$是最优估计值，是卡尔曼滤波的最终输出值。
$\overrightarrow{z_{0估}},{\mathrm{P}}_{0\mathrm{估}}$是人为设定的。