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

$郑重声明：该系列博客为本人(WenhaiZhu)独家私有,禁止转载与抄袭,首次举报有谢!$
 

一、前言

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

        卡尔曼滤波是基于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} 状态方程:{\check{x}}_k=f{\hat{x}}_{k-1}+q_{k}f为一维常数 \\ 观测方程:\hat{x}=h{\check{x}}_k+r_{k}h为一维常数\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_1$、$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$ 概率的概率总和吗?

三、公式推导

再推导之前，首先要明白已知条件与推导目的，避免计算过程中，都不知自己再做些什么了，如下：
$$已知:{\hat{x}}_{k-1}求解:{\hat{x}}_{k-1}\text{\hspace{1em}(09)}$$当然，仅仅凭其一个已知条件，是没有办法进行下去的，所以还需要额外的假设，以LG(线性高斯系统)为基础进行推断，故 $f_{X_k}^{-}(x)$、 $f_{X_k}^{+}(x)$、$f_{Q_k}(x)$ 、$f_{R_k}(x)$ 都符合正太正太分布：$$f(x)=\frac{1}{\sigma \sqrt{2\pi }}{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}X\sim N\left(\mu ,{\sigma }^2\right)\text{\hspace{1em}(10)}$$并且$$f_{Q_k}(x)\sim N(0,{\sigma }_{q_{k-1}})f_{R_k}(x)\sim N(0,{\sigma }_{R_{k-1}})\text{\hspace{1em}(11)}$$

其上的 $\mu$ 表示均值，$\sigma$ 表示标准差。除此之外，还需要对 (03) 式中状态转移方程与观测方程进行实例化，这里设：$$\begin{gathered} 状态方程:{\check{x}}_k=f{\hat{x}}_{k-1}+q_{k}f为一维常数 \\ 观测方程:\hat{x}=h{\check{x}}_k+r_{k}h为一维常数\text{\hspace{1em}(12)} \end{gathered}$$上面的假设都是比较简单的，$f$、$h$ 是两个参数，不会发生改变，$f$、$h$ 会改变的场景这里先不做考虑，其过程相对复杂一些，当然应用也会更加广泛。

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\text{\hspace{1em}(11)}$$设 $f_{Q_k}(x)$ 的均值为 ${\mu }_{Q_k}=0$，方差为 ${\sigma }_{Q_k}$；$f_{X_{k-1}}^{+}(x)$ 的均值为 ${\hat{x}}_{k-1}$，方差为 ${\sigma }_{X_{k-1}}^{+}$； 把他们带入上式得：
$$f_{X_{k-1}}^{+}(v)=\frac{1}{{\sigma }_{Xk-1}\sqrt{2\pi }}{e}^{-\frac{(v-{\hat{x}}_{k-1}{)}^2}{2{\sigma }_{Xk-1}^{-2}}}\text{\hspace{1em}(12)}$$$$f_{Q_{k-1}}(x-f(v))=\frac{1}{{\sigma }_{Qk-1}\sqrt{2\pi }}{e}^{-\frac{(x-av{)}^2}{2{\sigma }_{Qk-1}^2}}其中:f(v)=fv\text{\hspace{1em}(13)}$$
上面需要注意的是 ${\mu }_{Q_k}=0$，下面再把 $f_{X_{k-1}}^{+}(v)$ 与 $f_{Q_{k-1}}(x-f(v))$ 都带入到 (11) 式得：$$f_{X_k}^{-}(x)={\int }_{-\infty }^{+\infty }\frac{1}{{\sigma }_{Qk-1}\sqrt{2\pi }}{e}^{-\frac{(x-fv{)}^2}{2{\sigma }_{Qk-1}^2}}\cdot \frac{1}{{\sigma }_{Xk-1}^{+}\sqrt{2\pi }}{e}^{-\frac{(v-{\hat{x}}_{k-1}{)}^2}{2{\sigma }_{Xk-1}^{+2}}}\cdot \mathrm{d}v\text{\hspace{1em}(14)}$$ 此时，再来看上式，其实就比较明朗了，本质上就是前面提到的，两个高斯密度函数的乘积，使用 Mathematic 软件计算可以得到答案(后续会手动推导)，其中的变量 $v$ 会进行积分，积分之后就只剩下一个变量 $x$ ，得答案如下：
$$f_{X_k}^{-}(x)\sim N({\check{x}}_k,{\sigma }_{X_k}^{-})=N(f{\hat{x}}_{k-1},f^2{\sigma }_{X_{k-1}}^{+}+{\sigma }_{Q_{k-1}})\text{\hspace{1em}(15)}$$

2.更新步

完成先验概率密度函数 $f_{X_k}^{-}(x)$ 的推导，现在就是求似然概率密度函数，也就是一式中 $f_{X_k|Y_k}(x)=f_{R_k}\left[y_k-h(x)\right]$ 的结果了，不说其他的，直观看起来就简单太多了，直接参考前面的 (10) (11) 式，带入即可：
$$f_{X_k|Y_k}(x)=\frac{1}{{\sigma }_{R_k}\sqrt{2\pi }}{e}^{-\frac{(y-hx{)}^2}{2{\sigma }_{R_k}^2}}\text{\hspace{1em}(15)}$$都不用多说，其上很明显就是 $N(hx,{\sigma }_{Rk-1}^2)$ 的一个正太分布，现在需要利用他对前面的 $N({\mu }_{X_k}^{-},{\sigma }_{X_k}^{-2})$ 进行修正，得到 $N({\mu }_{X_k}^{+},{\sigma }_{X_k}^{+2})$，当然修正过程不能忘记还要乘一个归一化变量：
$${\eta }_k=[{\int }_{-\infty }^{+\infty }{f}_{X_k|Y_k}(x)\cdot f_{X_k}^{-}(x)dx{]}^{-1}\text{\hspace{1em}(16)}$$这里依旧直接给出答案，后续有时间再为大家详细推导，这里给出的是 $\hat{x}=E(f_{X_k}^{+}(x))$ ：
$$X_k^{+}=({\hat{x}}_k,{\sigma }_{X_k}^{+})\sim N\left(\frac{h{\sigma }_{X_k}^{-}{y}_k+{\sigma }_{R_k}{\check{x}}_k}{h^2{\sigma }_{X_k}^{-}+{\sigma }_{R_k}},\frac{{\sigma }_{R_k}{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+{\sigma }_{R_k}}\right)\text{\hspace{1em}(17)}$$

3.公式化简

虽然通过上面的推导，答案基本已经出来了，但是其对于编程的实现并不是很友好，所以还需要继续化简一下，在这之前我们先把上面的公式梳理一下，(15) (17) 式等价如下：
$${\check{x}}_k=f{\hat{x}}_{k-1}{\sigma }_{X_k}^{-}=f^2{\sigma }_{X_{k-1}}^{+}+{\sigma }_{Q_{k-1}}\text{\hspace{1em}(18)}$$$${\hat{x}}_k=\frac{h{\sigma }_{X_k}^{-}{y}_k+{\sigma }_{R_k}{\check{x}}_k}{h^2{\sigma }_{X_k}^{-}+{\sigma }_{R_k}}{\sigma }_{X_k}^{+}=\frac{{\sigma }_{R_k}{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+{\sigma }_{R_k}}\text{\hspace{1em}(19)}$$根据上面的式子可以很直观的知道，其是一个递归推导，若已知 ${\hat{x}}_0$ 与 ${\sigma }_{X_0}^{+}$，以及各个时刻的观测 $y_k$，则可递归推导：
$$【{\hat{x}}_0，{\sigma }_{X_0}^{+}，y_1】\to 【{\hat{x}}_1，{\sigma }_{X_1}^{+},y_2】\to \cdots \to 【{\hat{x}}_k，{\sigma }_{X_k}^{+}】\text{\hspace{1em}(20)}$$对于(19) 式分别化简如下：$$\begin{array}{rl}{\hat{x}}_k& =\frac{h{\sigma }_{X_k}^{-}{y}_k+{\sigma }_{R_k}{\check{x}}_k}{h^2{\sigma }_{X_k}^{-}+{\sigma }_{R_k}}\\ & =\frac{h{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+{\sigma }_{R_k}}{y}_k+\frac{{\sigma }_{R_k}{\check{x}}_k}{h^2{\sigma }_{X_k}^{-}+{\sigma }_{R_k}}\\ & =\frac{h{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+{\sigma }_{R_k}}{y}_k+\frac{\check{x}({\sigma }_{R_k}+h^2{\sigma }_{X_k}^{-})-{\check{x}}_k{h}^2{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+{\sigma }_{R_k}}\\ & =\frac{h{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+{\sigma }_{R_k}}{y}_k+\check{x}-\frac{\check{x}{h}^2{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+{\sigma }_{R_k}}\\ & =\frac{h{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+{\sigma }_{R_k}}(y_k-h\check{x})+\check{x}\end{array}\text{\hspace{1em}(21)}$$ $$\begin{array}{rl}{\sigma }_{X_k}^{+}& =\frac{{\sigma }_{R_k}{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+{\sigma }_{R_k}}\\ & =\frac{(h^2{\sigma }_{X_k}^{-}+{\sigma }_{R_k}){\sigma }_{X_k}^{-}-h^2{\sigma }_{X_k}^{-}{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+{\sigma }_{R_k}}\\ & ={\sigma }_{X_k}^{-}-\frac{h^2{\sigma }_{X_k}^{-}{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+{\sigma }_{R_k}}\\ & =(1-\frac{h{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+{\sigma }_{R_k}}h){\sigma }_{X_k}^{-}\end{array}\text{\hspace{1em}(22)}$$可以看到，其都存在共同的一项，那就是卡尔曼增益项： $$k_k=\frac{h{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+{\sigma }_{R_k}}\text{\hspace{1em}(23)}$$把其带如何到 (22) (23) 式可得：$${\hat{x}}_k=k_k(y_k-h\check{x})+\check{x}{\sigma }_{X_k}^{+}=(1-h{k}_k){\sigma }_{X_k}^{-}\text{\hspace{1em}(24)}$$

四、总结

通过上面一系列的推导，可以得到以下五个公式，其就是卡尔曼的五大核心公式，即 (15) 式中的两个公式(需展开)，以及 (23) (24)，整理如下：
$$①：{\check{x}}_k=f{\hat{x}}_{k-1}②：{\sigma }_{X_k}^{-}=f^2{\sigma }_{X_{k-1}}^{+}+{\sigma }_{Q_{k-1}}\text{\hspace{1em}(25)}$$ $$③：k_k=\frac{h{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+{\sigma }_{R_k}}\text{\hspace{1em}(26)}$$$$④：{\hat{x}}_k=k_k(y_k-h\check{x})+\check{x}⑤：{\sigma }_{X_k}^{+}=(1-h{k}_k){\sigma }_{X_k}^{-}\text{\hspace{1em}(27)}$$
上面的五个式子很明显是递推的若假设已知 ${\hat{x}}_0$、${\sigma }_{X_0}^{+}$、以及各个时刻观测 $y_k$，则可推导出出 ${\hat{x}}_k$、${\sigma }_{X_k}^{+}$，如下：
$$【{\hat{x}}_0,{\sigma }_{X_0}^{+},y_1】\to 【{\hat{x}}_1,{\sigma }_{X_1}^{+},y_2】\to \cdots \to 【{\hat{x}}_k,{\sigma }_{X_k}^{+}】\text{\hspace{1em}(28)}$$
不过上面推导的公式存在一个很大的问题，那就是其基于一维推导，实际应用过程中，很少使用到一维的卡尔曼滤波，虽然以这种方式进行推导，可以很好的理解整个推导过程，但是局限性太大。

另外，还有很多地方没有讲解清楚，如两个高斯分布的乘积与积分，高斯分布线性变化等，都未进行详细的讲解。不用着急，后续博客都会展开进行详细分析与推导。