卡尔曼家族从零解剖-(05)卡尔曼滤波→公式推导，应用通俗讲解，c++代码示例

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的 卡尔曼家族从零解剖 链接 :卡尔曼家族从零解剖-(00)目录最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/133846882
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

        在一篇博中，通过前面一系列博客，对卡尔滤波有了一定，且在上一篇博客中对整个流程进行了梳理。首先，贝叶斯滤波仅仅是一种思想指导，其是没有办法直接应用的，其中卡尔曼漫滤就一种实例化体现，当然，也并非完全实例化，若想卡尔漫滤波算法落地，则需要再进补一步根据实际应用情况， 进行建模.

        卡尔曼滤波是基于LG(线性高斯)系统进行推断，故其只适用于LG 数学模形，若想适用于 NL 或 NG 系统，则需要使用共它变种算法，如EKF(扩展卡尔曼滤波)，这些内容在后面的博客中会进行详细的分析.上一片篇空中， 总结出公式如下：
$$f_{X_k}^{+}(x)={\eta }_k\cdot f_{X_k|Y_k}(x)\cdot f_{X_k}^{-}(x)={\eta }_k\cdot f_{R_k}\left[y_k-h(x)\right]\cdot {\int }_{-\infty }^{+\infty }{f}_{Q_k}[x-f(v)]f_{X_{k-1}}^{+}(v)\mathrm{d}v\text{\hspace{1em}(01)}$$$${\eta }_k=[{\int }_{-\infty }^{+\infty }(f_{R_k}\left[y_k-h(x)\right]\cdot {\int }_{-\infty }^{+\infty }{f}_{Q_k}[x-f(v)]f_{X_{k-1}}^{+}(v)\mathrm{d}v\mathrm{d}x{]}^{-1})\mathrm{d}x\text{\hspace{1em}(02)}$$已经分析过，阻碍算法落地罪魁祸首的就是上面的两个无穷积分，那么接下来的核心，就是如何避免无穷积分，或者直接求解无穷积分了。现在来看，尔麦滤波关是如何从根源上避免无穷积，其首相如下做没：

$(01)$ 状态转移函数 $f(x)$ 与 观测函数$h(x)$ 都是线性的，如下( $\check{.}$ 表示先验，$\widehat{.}$ 表示后验 )：$$\begin{gathered} f({\check{x}}_k)=af({\hat{x}}_{k-1})+q_{k}a为一维常数 \\ h(\hat{x})=bf({\check{x}}_k)+r_{k}b为一维常数\text{\hspace{1em}(03)} \end{gathered}$$$(02)$ 其上 $q_k\in Q\sim N(u_{q_k},{\sigma }_k^2)$， $r_k\in R\sim N(u_{r_k},{\sigma }_{r_k}^2)$，为正太分布。

需要注意，后续推导过程先一维示例，再拓展到高维，因为一维不设计到矩阵、多元高斯、协方差矩阵等。所谓通过现象看本质。虽然实际应用中很少使用一维的，但是从一维来理解最合适最底层原理还是比较合适的。因为是一维的，所以使用小写字母表示，另外上面假设是随机过程的实例化，也就是 ${\check{x}}_k$ 、${\check{x}}_k$、$q_k$、$r_k$ 表示随机过程的具体取值，而非随机变量。另外，公式 (02) 推导使用到假设：

$(03):$ $X_0$ 与 $Q_2$、$Q_2$、$Q_3$、$......$、$Q_k$ 相互独立。
$(04):$ $X_1$ 与 $R_1$、$R_2$、$R_3$、$......$、$R_k$ 相互独立。

所以这里得记录一下，也就是实际应用过程中，不能把这个假设忽略了，否则是不适用于贝叶斯滤波得。除了上面的假设，还需要额外的知识点：

$(05):$ 正太分布函数进行线性变换，依旧为符合正太分布。
$(06):$ 两个正太分布的乘积依旧为正太分布(先记住结论后续进行推导)。

两正太分布乘积结果如下(注意，先记结果，别纠结过程，陷入死胡同了)：
$$x_1\in X_1\sim N(v_1,{\sigma }_1^2)x_2\in X_2\sim N(v_2,{\sigma }_2^2)\text{\hspace{1em}(04)}$$$$f(x_1)\ast f(x_2)=N\left(\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}{\mu }_2+\frac{{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}{\mu }_1，\frac{{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}\right)\text{\hspace{1em}(05)}$$

二、思想指导

首先要整体来分析一下，假设先验状态 ${\hat{x}}_{k-1}$ 的概率密度函数 $f_{k-1}^{+}$ 符合正太分布 $N(v_{x_{k-1}},{\sigma }_{x_{k-1}}^2)$，那么根据状态转移函数 $f({\check{x}}_k)=af({\hat{x}}_{k-1})+q_k$，结合 【$(05):$ 正太分布函数进行线性变换，依旧为符合正太分布。 可以知道 $f({\check{x}}_k)$ 】，可知 $f({\check{x}}_k)=f_{X_k}^{+}(x)$ 也符合正太分布，直白的说，若 $k-1$ 时刻后验符合正太分布，则 $k$ 时刻的先验也符合正太分布，再来看 (01) 式：$$f_{X_k}^{+}(x)={\eta }_k\cdot f_{X_k|Y_k}(x)\cdot f_{X_k}^{-}(x)\text{\hspace{1em}(06)}$$ 可以知道，若 $f_{X_k|Y_k}(x)$ 也符合正太分布，那么 $f_{X_k}^{+}(x)$ 也符合正太分布了(${\eta }_k$为一个常数)，这样递推公式就出来了。根据 (1) 式子可知：$$f_{X_k|Y_k}(x)=f_{R_k}\left[y_k-h(x)\right]\text{\hspace{1em}(07)}$$ 上式中由于 $h(x)$ 是线性函数， $y_k-h(x)$ 的作用相当于对原本的太正分布图形进行了平移或缩放，故结果 $f_{X_k|Y_k}(x)$ 依旧为正太分布。

$核心:$ 根据上面的推导，可知，若 $f_{k-1}^{+}$ 符合正太分布，则 $f_{X_k}^{-}(x)$ 与 $f_{X_k|Y_k}(x)$ 都符合正太，又 ${\eta }_k$ 为一参数，根据 【$(06):$ 两个正太分布的乘积依旧为正太分布】，可知 $f_{X_k}^{+}(x)$ 为正太分布。最后可知，若 $x_0$ 符合正太分布，可以一直递推出 ${\hat{x}}_1$、${\hat{x}}_2$、$\cdots$、${\hat{x}}_{k-1}$、${\hat{x}}_k$ 都是符合正太分布。

这里额外补充一下 $\eta$ 是一个常数，其表达式在 卡尔曼家族从零解剖-(01)预备知识点 中推导过：$$\eta =[f_{Y_k}(x){]}^{-1}=[{\int }_{-\infty }^{+\infty }{f}_{Y_k\mid X_k}(y_k\mid x)f_X(x)\mathrm{d}x{]}^{-1}\text{\hspace{1em}(08)}$$其本质来源于连续随机变量的全概率公式，其实也比较好理解，目前观测到 $y_k$，那么 $P(y_k)$ 已经能被确定的，因为已经假设 $f_{Y_k}(X)$ 符合正太分布。观测到 $y_k$ 的总概率，不就是所有所有 $x$ 状态下，能够观测到 $Y_k$ 概率的概率总和吗?

三、公式推导

1.先验推导

首先参考(01)式，可以指导 $f_{X_{k-1}}^{-}$ 与 $f_{X_{k-1}}^{+}$ 的关系，如下所示：
$$f_{X_k}^{-}(x)={\int }_{-\infty }^{+\infty }{f}_{Q_k}[x-f(v)]f_{X_{k-1}}^{+}(v)\mathrm{d}v\mathrm{d}x{]}^{-1})\mathrm{d}x\text{\hspace{1em}(09)}$$

2.似然推导

3.后验推导