卡尔曼家族从零解剖-(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 ) = ηk⋅ f Xk∣ Yk ( x ) ⋅ f X k−( x ) = ηk⋅ f R k[ y k−h(x)]⋅ ∫ − ∞ + ∞f Q k[ x − f ( v ) ] f X k−1 +( v ) d v(01) \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 fXk​+​(x)=ηk​⋅fXk​∣Yk​​(x)⋅fXk​−​(x)=ηk​⋅fRk​​[yk​−h(x)]⋅∫−∞+∞​fQk​​[x−f(v)]fXk−1​+​(v)dv(01)ηk= [ ∫ − ∞ + ∞( f R k[ y k−h(x)]⋅ ∫ − ∞ + ∞f Q k[ x − f ( v ) ] f X k−1 +( v ) d v d x ] − 1) d x(02) \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 ηk​=[∫−∞+∞​(fRk​​[yk​−h(x)]⋅∫−∞+∞​fQk​​[x−f(v)]fXk−1​+​(v)dvdx]−1)dx(02)已经分析过，阻碍算法落地罪魁祸首的就是上面的两个无穷积分，那么接下来的核心，就是如何避免无穷积分，或者直接求解无穷积分了。现在来看，尔麦滤波关是如何从根源上避免无穷积，其首相如下做没：

$(01)$ 状态转移函数 $f(x)$ 与 观测函数$h(x)$ 都是线性的，如下( .ˇ \check{.}.ˇ 表示先验， .^ \hat{.}.^ 表示后验 )：f (x ˇk) = a f (x ^ k − 1) + qk a 为一维常数 h ( x^) = b f (x ˇk) + rk b 为一维常数(03) \color{Green} \tag{03} f(\check x_{k})=af(\hat x_{k-1})+q_k~~~~~~~~~a为一维常数\\h(\hat x)=bf(\check x_k)+r_k~~~~~~~~~~~~~~b为一维常数 f(xˇk​)=af(x^k−1​)+qk​a为一维常数h(x^)=bf(xˇk​)+rk​b为一维常数(03)$(02)$ 其上 q k∈Q∼N( u qk , σ k 2) \color{Green} q_k \in Q \sim N(u_{q_k},\sigma^2_{k})qk​∈Q∼N(uqk​​,σk2​)， r k∈R∼N( u rk , σ rk2) \color{Green} r_k \in R \sim N(u_{r_k},\sigma^2_{r_k})rk​∈R∼N(urk​​,σrk​2​)，为正太分布。

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

$(03):$ X0 X_0X0​ 与 Q2 Q_2Q2​、 Q2 Q_2Q2​、 Q3 Q_3Q3​、$\dots \dots$、 Qk Q_{k}Qk​ 相互独立。
$(04):$ X1 X_1X1​ 与 R1 R_1R1​、 R2 R_2R2​、 R3 R_3R3​、$\dots \dots$、 Rk R_{k}Rk​ 相互独立。

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

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

两正太分布乘积结果如下(注意，先记结果，别纠结过程，陷入死胡同了)：
x1∈ X1∼ N ( v1, σ12)x2∈ X2∼ N ( v2, σ22)(04) \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) x1​∈X1​∼N(v1​,σ12​)x2​∈X2​∼N(v2​,σ22​)(04)f ( x1) ∗ f ( x2) = N ( σ12 σ 1 2+ σ 2 2μ 2+ σ22 σ 1 2+ σ 2 2μ 1，σ 1 2 σ 2 2 σ 1 2+ σ 2 2 ) (05) \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) f(x1​)∗f(x2​)=N(σ12​+σ22​σ12​​μ2​+σ12​+σ22​σ22​​μ1​，σ12​+σ22​σ12​σ22​​)(05)

二、思想指导

首先要整体来分析一下，假设先验状态 x ^ k − 1 \hat x_{k-1}x^k−1​ 的概率密度函数 f k − 1+ f_{k-1}^+fk−1+​ 符合正太分布 N ( v x k−1 , σ x k−1 2)N(v_{x_{k-1}},\sigma _{x_{k-1}}^2)N(vxk−1​​,σxk−1​2​)，那么根据状态转移函数 f (x ˇk) = a f (x ^ k − 1) + qk f(\check x_{k})=af(\hat x_{k-1})+q_kf(xˇk​)=af(x^k−1​)+qk​，结合 【$(05):$ 正太分布函数进行线性变换，依旧为符合正太分布。 可以知道 f (x ˇk)f(\check x_{k})f(xˇk​) 】，可知 f (x ˇk) = f X k+( x )f(\check x_{k})=f^+_{X_k}(x)f(xˇk​)=fXk​+​(x) 也符合正太分布，直白的说，若 $k-1$ 时刻后验符合正太分布，则 $k$ 时刻的先验也符合正太分布，再来看 (01) 式：f X k+( x ) = ηk⋅ f Xk∣ Yk ( x ) ⋅ f X k−( x )(06) \color{Green} \tag{06} f_{X_k}^+(x)=\eta_k ·f_{X_k | Y_k}(x) ·f_{X_k}^-(x) fXk​+​(x)=ηk​⋅fXk​∣Yk​​(x)⋅fXk​−​(x)(06) 可以知道，若 f Xk∣ Yk ( x )f_{X_k | Y_k}(x)fXk​∣Yk​​(x) 也符合正太分布，那么 f X k+( x )f_{X_k}^+(x)fXk​+​(x) 也符合正太分布了( ηk \eta_kηk​为一个常数)，这样递推公式就出来了。根据 (1) 式子可知：f Xk∣ Yk ( x ) = f R k[ y k−h(x)] (07) \color{Green} \tag{07} f_{X_k | Y_k}(x)=f_{R_{k}}\left[y_{k}-h(x)\right] fXk​∣Yk​​(x)=fRk​​[yk​−h(x)](07) 上式中由于 $h(x)$ 是线性函数， yk− h ( x )y_{k}-h(x)yk​−h(x) 的作用相当于对原本的太正分布图形进行了平移或缩放，故结果 f Xk∣ Yk ( x )f_{X_k | Y_k}(x)fXk​∣Yk​​(x) 依旧为正太分布。

$核心:$ 根据上面的推导，可知，若 f k − 1+ f_{k-1}^+fk−1+​ 符合正太分布，则 f X k−( x )f_{X_k}^-(x)fXk​−​(x) 与 f Xk∣ Yk ( x )f_{X_k | Y_k}(x)fXk​∣Yk​​(x) 都符合正太，又 ηk \eta_kηk​ 为一参数，根据 【$(06):$ 两个正太分布的乘积依旧为正太分布】，可知 f X k+( x )f_{X_k}^+(x)fXk​+​(x) 为正太分布。最后可知，若 x0 x_0x0​ 符合正太分布，可以一直递推出 x ^1 \hat x_1x^1​、x ^2 \hat x_2x^2​、$\cdots$、x ^ k − 1 \hat x_{k-1}x^k−1​、x ^k \hat x_{k}x^k​ 都是符合正太分布。

这里额外补充一下 $\eta$ 是一个常数，其表达式在 卡尔曼家族从零解剖-(01)预备知识点 中推导过：η = [ f Y k( x ) ] − 1= [∫ −∞+∞fY k∣ X k ( y k∣x) f X(x)dx] − 1 (08) \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} η=[fYk​​(x)]−1=[∫−∞+∞​fYk​∣Xk​​(yk​∣x)fX​(x)dx]−1(08)其本质来源于连续随机变量的全概率公式，其实也比较好理解，目前观测到 yk y_kyk​，那么 P ( yk)P(y_k)P(yk​) 已经能被确定的，因为已经假设 f Y k( X )f_{Y_k}(X)fYk​​(X) 符合正太分布。观测到 yk y_kyk​ 的总概率，不就是所有所有 $x$ 状态下，能够观测到 Yk Y_kYk​ 概率的概率总和吗?

三、公式推导

1.先验推导

首先参考(01)式，可以指导 f X k−1 − f_{X_{k-1}}^-fXk−1​−​ 与 f X k−1 + f_{X_{k-1}}^+fXk−1​+​ 的关系，如下所示：
f X k−( x ) = ∫ − ∞ + ∞f Q k[ x − f ( v ) ] f X k−1 +( v ) d v d x ] − 1) d x(09) \color{Green} \tag{09} 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 fXk​−​(x)=∫−∞+∞​fQk​​[x−f(v)]fXk−1​+​(v)dvdx]−1)dx(09)

2.似然推导

3.后验推导