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

这篇具有很好参考价值的文章主要介绍了卡尔曼家族从零解剖-(05)卡尔曼滤波→公式推导，应用通俗讲解，c++代码示例。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的卡尔曼家族从零解剖链接 :卡尔曼家族从零解剖-(00)目录最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/133846882

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

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

一、前言

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

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

$\color{blue}(01)$ 状态转移函数 $f (x)$ 与观测函数 $h (x)$ 都是线性的，如下( $\check{.}$ 表示先验， $\hat{.}$ 表示后验 )： $\color{Green} \tag{03} 状态方程:~~~~~~ \check x_{k}=f\hat x_{k-1}+q_k~~~~~~~~~f为一维常数\\观测方程:~~~~~~~\hat x=h\check x_k+r_k~~~~~~~~~~~~~~h为一维常数$ $\color{blue}(02)$ 其上 $\color{Green} q_k \in Q \sim N(u_{q_k},\sigma^2_{k})$ ， $\color{Green} r_k \in R \sim N(u_{r_k},\sigma^2_{r_k})$ ，为正太分布。

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

$\color{blue}(03):$ $X_0$ 与 $Q_1$ 、 $Q_2$ 、 $Q_3$ 、 $......$ 、 $Q_{k}$ 相互独立。
$\color{blue}(04):$ $X_1$ 与 $R_1$ 、 $R_2$ 、 $R_3$ 、 $......$ 、 $R_{k}$ 相互独立。

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

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

两正太分布乘积结果如下(注意，先记结果，别纠结过程，陷入死胡同了)：
$\color{Green} \tag{04} x_1 \in X_1 \sim N(v_1,\sigma_1^2)~~~~~~~~~~~~~~~~~~~~x_2 \in X_2 \sim N(v_2,\sigma_2^2)$ $\color{Green} \tag{05} f(x_1)*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)$

二、思想指导

首先要整体来分析一下，假设先验状态 $\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$ ，结合【 $\color{blue}(05):$ 正太分布函数进行线性变换，依旧为符合正太分布。可以知道 $f(\check x_{k})$ 】，可知 $f(\check x_{k})=f^+_{X_k}(x)$ 也符合正太分布，直白的说，若 $k - 1$ 时刻后验符合正太分布，则 $k$ 时刻的先验也符合正太分布，再来看 (01) 式： $\color{Green} \tag{06} f_{X_k}^+(x)=\eta_k ·f_{X_k | Y_k}(x) ·f_{X_k}^-(x)$ 可以知道，若 $f_{X_k | Y_k}(x)$ 也符合正太分布，那么 $f_{X_k}^+(x)$ 也符合正太分布了( $\eta_k$ 为一个常数)，这样递推公式就出来了。根据 (1) 式子可知： $\color{Green} \tag{07} f_{X_k | Y_k}(x)=f_{R_{k}}\left[y_{k}-h(x)\right]$ 上式中由于 $h (x)$ 是线性函数， $y_{k}-h(x)$ 的作用相当于对原本的太正分布图形进行了平移或缩放，故结果 $f_{X_k | Y_k}(x)$ 依旧为正太分布。

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

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

三、公式推导

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

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

1.预测步

首先参考(01)式，可以指导 $f_{X_{k-1}}^-$ 与 $f_{X_{k-1}}^+$ 的关系，如下所示：
$\color{Green} \tag{11} f_{X_k}^-(x) =\int_{-\infty}^{+\infty} f_{Q_{k}}[x-f(v)] f_{X_{k-1}}^{+}(v) \mathrm{d} v$ 设 $f_{Q_{k}}(x)$ 的均值为 $\color {red} \mu_{Q_{k}}=0$ ，方差为 $\sigma_{Q_{k}}$ ； $f_{X_{k-1}}^+(x)$ 的均值为 $\color {red} \hat x_{k-1}$ ，方差为 $\sigma_{X_{k-1}}^+$ ；把他们带入上式得：
$\color{Green} \tag{12} f_{X_{k-1}}^{+}(v)=\frac{1}{ \sigma_{X{k-1}} \sqrt{2 \pi}} e^{-\frac{(v-\hat x_{k-1})^{2}}{2 \sigma_{X{k-1}}^{-2}}}$ $\color{Green} \tag{13} f_{Q_{k-1}}(x-f(v))=\frac{1}{ \sigma_{Q{k-1}} \sqrt{2 \pi}} e^{-\frac{(x-av)^{2}}{2 \sigma_{Q{k-1}}^{2}}} ~~~~~~~~其中: f(v)=fv$
上面需要注意的是 $\color {red} \mu_{Q_{k}}=0$ ，下面再把 $f_{X_{k-1}}^{+}(v)$ 与 $f_{Q_{k-1}}(x-f(v))$ 都带入到 (11) 式得： $\color{Green} \tag{14} f_{X_k}^-(x) =\int_{-\infty}^{+\infty} \frac{1}{ \sigma_{Q{k-1}} \sqrt{2 \pi}} e^{-\frac{(x-fv)^{2}}{2 \sigma_{Q{k-1}}^{2}}} · \frac{1}{ \sigma_{X{k-1}}^+ \sqrt{2 \pi}} e^{-\frac{(v-\hat x_{k-1})^{2}}{2 \sigma_{X{k-1}}^{+2}}} · \mathrm{d} v$ 此时，再来看上式，其实就比较明朗了，本质上就是前面提到的，两个高斯密度函数的乘积，使用 Mathematic 软件计算可以得到答案(后续会手动推导)，其中的变量 $v$ 会进行积分，积分之后就只剩下一个变量 $x$ ，得答案如下：
$\color{red} \tag{15} 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}})$

2.更新步

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

3.公式化简

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

四、总结

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