卡尔曼滤波公式总结

引入一个离散控制过程系统：
$$X_{(k)}=A{X}_{(k-1)}+B{U}_{(k)}+W_{(k)}$$

系统测量值：
$$Z_{(k)}=H{X}_{(k)}+V_{(k)}$$
卡尔曼5个核心公式：

基于上一时刻进行的计算：
$$系统状态值:X_{(k|k-1)}=A{X}_{(k-1|k-1)}+B{U}_{(k)}\text{\hspace{1em}(1)}$$

$$系统协方差:P_{(k|k-1)}=A{P}_{(k-1|k-1)}{A}^{\prime }+Q\text{\hspace{1em}(2)}$$

然后对当前时刻进行更新：

$$X_{(k|k)}=X_{(k|k-1)}+K{g}_{(k)}(Z_{(k)}-H{X}_{(k|k-1)})\text{\hspace{1em}(3)}$$

$$其中K{g}_{(k)}=\frac{P_{(k|k-1)}H’}{H{P}_{(k|k-1)}H’+R}\text{\hspace{1em}(4)}$$

$$P_{(k|k)}=(I-K{g}_{(k)}H)P_{(k|k-1)}\text{\hspace{1em}(5)}$$