讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的 卡尔曼家族从零解剖 链接 :卡尔曼家族从零解剖-(00)目录最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/133846882
文末正下方中心提供了本人 联系方式， 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证
郑重声明：该系列博客为本人 ( W e n h a i Z h u ) 独家私有 , 禁止转载与抄袭 , 首次举报有谢 ! \color{red}郑重声明：该系列博客为本人(WenhaiZhu)独家私有,禁止转载与抄袭,首次举报有谢! 郑重声明：该系列博客为本人(WenhaiZhu)独家私有,禁止转载与抄袭,首次举报有谢!
一、前言
在一篇博中，通过前面一系列博客，对卡尔滤波有了一定，且在上一篇博客中对整个流程进行了梳理。首先，贝叶斯滤波仅仅是一种思想指导，其是没有办法直接应用的，其中卡尔曼漫滤就一种实例化体现，当然，也并非完全实例化，若想卡尔漫滤波算法落地，则需要再进补一步根据实际应用情况， 进行建模.
卡尔曼滤波是基于LG(线性高斯)系统进行推断，故其只适用于LG 数学模形，若想适用于 NL 或 NG 系统，则需要使用共它变种算法，如EKF(扩展卡尔曼滤波)，这些内容在后面的博客中会进行详细的分析.上一片篇空中， 总结出公式如下：
f X k + ( x ) = η k ⋅ f X k ∣ Y k ( 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 ) \color{blue}(01) (01) 状态转移函数 f ( x ) f(x) f(x) 与 观测函数 h ( x ) h(x) h(x) 都是线性的，如下( . ˇ \check{.} .ˇ 表示先验， . ^ \hat{.} .^ 表示后验 )： 状态方程 : x ˇ k = f x ^ k − 1 + q k f 为一维常数 观测方程 : x ^ = h x ˇ k + r k h 为一维常数 (03) \color{Green} \tag{03} 状态方程:~~~~~~ \check x_{k}=f\hat x_{k-1}+q_k~~~~~~~~~f为一维常数\\观测方程:~~~~~~~\hat x=h\check x_k+r_k~~~~~~~~~~~~~~h为一维常数 状态方程: xˇk=fx^k−1+qk f为一维常数观测方程: x^=hxˇk+rk h为一维常数(03) ( 02 ) \color{blue}(02) (02) 其上 q k ∈ Q ∼ N ( u q k , σ 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 r k , σ r k 2 ) \color{Green} r_k \in R \sim N(u_{r_k},\sigma^2_{r_k}) rk∈R∼N(urk,σrk2)，为正太分布。
需要注意，后续推导过程先一维示例，再拓展到高维，因为一维不设计到矩阵、多元高斯、协方差矩阵等。所谓通过现象看本质。虽然实际应用中很少使用一维的，但是从一维来理解最合适最底层原理还是比较合适的。因为是一维的，所以使用小写字母表示，另外上面假设是随机过程的实例化，也就是 x ˇ k \check x_{k} xˇk 、 x ˇ k \check x_k xˇk、 q k q_k qk、 r k r_k rk 表示随机过程的具体取值，而非随机变量。另外，公式 (02) 推导使用到假设：
( 03 ) : \color{blue}(03): (03): X 0 X_0 X0 与 Q 1 Q_1 Q1、 Q 2 Q_2 Q2、 Q 3 Q_3 Q3、 . . . . . . ...... ......、 Q k Q_{k} Qk 相互独立。
( 04 ) : \color{blue}(04): (04): X 1 X_1 X1 与 R 1 R_1 R1、 R 2 R_2 R2、 R 3 R_3 R3、 . . . . . . ...... ......、 R k R_{k} Rk 相互独立。
所以这里得记录一下，也就是实际应用过程中，不能把这个假设忽略了，否则是不适用于贝叶斯滤波得。除了上面的假设，还需要额外的知识点：
( 05 ) : \color{blue}(05): (05): 正太分布函数进行线性变换，依旧为符合正太分布。
( 06 ) : \color{blue}(06): (06): 两个正太分布的乘积依旧为正太分布(先记住结论后续进行推导)。
两正太分布乘积结果如下(注意，先记结果，别纠结过程，陷入死胡同了)：
x 1 ∈ X 1 ∼ N ( v 1 , σ 1 2 ) x 2 ∈ X 2 ∼ N ( v 2 , σ 2 2 ) (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 ( x 1 ) ∗ f ( x 2 ) = N ( σ 1 2 σ 1 2 + σ 2 2 μ 2 + σ 2 2 σ 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−12)，那么根据状态转移函数 f ( x ˇ k ) = a f ( x ^ k − 1 ) + q k f(\check x_{k})=af(\hat x_{k-1})+q_k f(xˇk)=af(x^k−1)+qk，结合 【 ( 05 ) : \color{blue}(05): (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-1 k−1 时刻后验符合正太分布，则 k k k 时刻的先验也符合正太分布，再来看 (01) 式： f X k + ( x ) = η k ⋅ f X k ∣ Y k ( 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 X k ∣ Y k ( 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 X k ∣ Y k ( 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 ) h(x) h(x) 是线性函数， y k − h ( x ) y_{k}-h(x) yk−h(x) 的作用相当于对原本的太正分布图形进行了平移或缩放，故结果 f X k ∣ Y k ( x ) f_{X_k | Y_k}(x) fXk∣Yk(x) 依旧为正太分布。
核心 : \color{red}核心: 核心: 根据上面的推导，可知，若 f k − 1 + f_{k-1}^+ fk−1+ 符合正太分布，则 f X k − ( x ) f_{X_k}^-(x) fXk−(x) 与 f X k ∣ Y k ( x ) f_{X_k | Y_k}(x) fXk∣Yk(x) 都符合正太，又 η k \eta_k ηk 为一参数，根据 【 ( 06 ) : \color{blue}(06): (06): 两个正太分布的乘积依旧为正太分布】，可知 f X k + ( x ) f_{X_k}^+(x) fXk+(x) 为正太分布。最后可知，若 x 0 x_0 x0 符合正太分布，可以一直递推出 x ^ 1 \hat x_1 x^1、 x ^ 2 \hat x_2 x^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 = [ ∫ − ∞ + ∞ f Y k ∣ X k ( y k ∣ x ) f X ( x ) d x ] − 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)其本质来源于连续随机变量的全概率公式，其实也比较好理解，目前观测到 y k y_k yk，那么 P ( y k ) P(y_k) P(yk) 已经能被确定的，因为已经假设 f Y k ( X ) f_{Y_k}(X) fYk(X) 符合正太分布。观测到 y k y_k yk 的总概率，不就是所有所有 x x x 状态下，能够观测到 Y k Y_k Yk 概率的概率总和吗?
三、公式推导
再推导之前，首先要明白已知条件与推导目的，避免计算过程中，都不知自己再做些什么了，如下：
已知 : x ^ k − 1 求解 : x ^ k − 1 (09) \color{Green} \tag{09}已知: \hat x_{k-1} ~~~~~~~~~~~~~~~~~~求解:\hat x_{k-1} 已知:x^k−1 求解:x^k−1(09)当然，仅仅凭其一个已知条件，是没有办法进行下去的，所以还需要额外的假设，以LG(线性高斯系统)为基础进行推断，故 f X k − ( x ) f_{X_{k}}^-(x) fXk−(x)、 f X k + ( x ) f_{X_{k}}^+(x) fXk+(x)、 f Q k ( x ) f_{Q_{k}}(x) fQk(x) 、 f R k ( x ) f_{R_{k}}(x) fRk(x) 都符合正太正太分布： f ( x ) = 1 σ 2 π e − ( x − μ ) 2 2 σ 2 X ∼ N ( μ , σ 2 ) (10) \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) f(x)=σ2π1e−2σ2(x−μ)2 X∼N(μ,σ2)(10)并且 f Q k ( x ) ∼ N ( 0 , σ q k − 1 ) f R k ( x ) ∼ N ( 0 , σ R k − 1 ) (11) \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}}) fQk(x)∼N(0,σqk−1) fRk(x)∼N(0,σRk−1)(11)
其上的 μ \mu μ 表示均值， σ \sigma σ 表示标准差。除此之外，还需要对 (03) 式中状态转移方程与观测方程进行实例化，这里设： 状态方程 : x ˇ k = f x ^ k − 1 + q k f 为一维常数 观测方程 : x ^ = h x ˇ k + r k h 为一维常数 (12) \color{Green} \tag{12} 状态方程:~~~~~~ \check x_{k}=f\hat x_{k-1}+q_k~~~~~~~~~f为一维常数\\观测方程:~~~~~~~\hat x=h\check x_k+r_k~~~~~~~~~~~~~~h为一维常数 状态方程: xˇk=fx^k−1+qk f为一维常数观测方程: x^=hxˇk+rk h为一维常数(12)上面的假设都是比较简单的， f f f、 h h h 是两个参数，不会发生改变， f f f、 h h h 会改变的场景这里先不做考虑，其过程相对复杂一些，当然应用也会更加广泛。
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 (11) \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 fXk−(x)=∫−∞+∞fQk[x−f(v)]fXk−1+(v)dv(11)设 f Q k ( x ) f_{Q_{k}}(x) fQk(x) 的均值为 μ Q k = 0 \color {red} \mu_{Q_{k}}=0 μQk=0，方差为 σ Q k \sigma_{Q_{k}} σQk； f X k − 1 + ( x ) f_{X_{k-1}}^+(x) fXk−1+(x) 的均值为 x ^ k − 1 \color {red} \hat x_{k-1} x^k−1，方差为 σ X k − 1 + \sigma_{X_{k-1}}^+ σXk−1+； 把他们带入上式得：
f X k − 1 + ( v ) = 1 σ X k − 1 2 π e − ( v − x ^ k − 1 ) 2 2 σ X k − 1 − 2 (12) \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}}} fXk−1+(v)=σXk−12π1e−2σXk−1−2(v−x^k−1)2(12) f Q k − 1 ( x − f ( v ) ) = 1 σ Q k − 1 2 π e − ( x − a v ) 2 2 σ Q k − 1 2 其中 : f ( v ) = f v (13) \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 fQk−1(x−f(v))=σQk−12π1e−2σQk−12(x−av)2 其中:f(v)=fv(13)
上面需要注意的是 μ Q k = 0 \color {red} \mu_{Q_{k}}=0 μQk=0，下面再把 f X k − 1 + ( v ) f_{X_{k-1}}^{+}(v) fXk−1+(v) 与 f Q k − 1 ( x − f ( v ) ) f_{Q_{k-1}}(x-f(v)) fQk−1(x−f(v)) 都带入到 (11) 式得： f X k − ( x ) = ∫ − ∞ + ∞ 1 σ Q k − 1 2 π e − ( x − f v ) 2 2 σ Q k − 1 2 ⋅ 1 σ X k − 1 + 2 π e − ( v − x ^ k − 1 ) 2 2 σ X k − 1 + 2 ⋅ d v (14) \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 fXk−(x)=∫−∞+∞σQk−12π1e−2σQk−12(x−fv)2⋅σXk−1+2π1e−2σXk−1+2(v−x^k−1)2⋅dv(14) 此时，再来看上式，其实就比较明朗了，本质上就是前面提到的，两个高斯密度函数的乘积，使用 Mathematic 软件计算可以得到答案(后续会手动推导)，其中的变量 v v v 会进行积分，积分之后就只剩下一个变量 x x x ，得答案如下：
f X k − ( x ) ∼ N ( x ˇ k , σ X k − ) = N ( f x ^ k − 1 , f 2 σ X k − 1 + + σ Q k − 1 ) (15) \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}}) fXk−(x)∼N(xˇk,σXk−)=N(fx^k−1,f2σXk−1++σQk−1)(15)
2.更新步
完成先验概率密度函数 f X k − ( x ) f_{X_k}^-(x) fXk−(x) 的推导，现在就是求似然概率密度函数，也就是一式中 f X k ∣ Y k ( x ) = f R k [ y k − h ( x ) ] f_{X_k | Y_k}(x)=f_{R_{k}}\left[y_{k}-h(x)\right] fXk∣Yk(x)=fRk[yk−h(x)] 的结果了，不说其他的，直观看起来就简单太多了，直接参考前面的 (10) (11) 式，带入即可：
f X k ∣ Y k ( x ) = 1 σ R k 2 π e − ( y − h x ) 2 2 σ R k 2 (15) \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}}} fXk∣Yk(x)=σRk2π1e−2σRk2(y−hx)2(15)都不用多说，其上很明显就是 N ( h x , σ R k − 1 2 ) N (hx,\sigma_{R{k-1}}^2) N(hx,σRk−12) 的一个正太分布，现在需要利用他对前面的 N ( μ X k − , σ X k − 2 ) N(\mu_{X_{k}}^-,\sigma^{-2}_{X_{k}}) N(μXk−,σXk−2) 进行修正，得到 N ( μ X k + , σ X k + 2 ) N(\mu_{X_{k}}^+,\sigma^{+2}_{X_{k}}) N(μXk+,σXk+2)，当然修正过程不能忘记还要乘一个归一化变量：
η k = [ ∫ − ∞ + ∞ f X k ∣ Y k ( x ) ⋅ f X k − ( x ) d x ] − 1 (16) \color{Green} \tag{16} \eta_k=[\int_{-\infty}^{+\infty}f_{X_k | Y_k}(x)·f_{X_k}^-(x)dx]^{-1} ηk=[∫−∞+∞fXk∣Yk(x)⋅fXk−(x)dx]−1(16)这里依旧直接给出答案，后续有时间再为大家详细推导，这里给出的是 x ^ = E ( f X k + ( x ) ) \hat x= E(f_{X_k}^+(x)) x^=E(fXk+(x)) ：
X k + = ( x ^ k , σ X k + ) ∼ N ( h σ X k − y k + σ R k x ˇ k h 2 σ X k − + σ R k , σ R k σ X k − h 2 σ X k − + σ R k ) (17) \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) Xk+=(x^k,σXk+)∼N(h2σXk−+σRkhσXk−yk+σRkxˇk,h2σXk−+σRkσRkσXk−)(17)
3.公式化简
虽然通过上面的推导，答案基本已经出来了，但是其对于编程的实现并不是很友好，所以还需要继续化简一下，在这之前我们先把上面的公式梳理一下，(15) (17) 式等价如下：
x ˇ k = f x ^ k − 1 σ X k − = f 2 σ X k − 1 + + σ Q k − 1 (18) \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}} xˇk=fx^k−1 σXk−=f2σXk−1++σQk−1(18) x ^ k = h σ X k − y k + σ R k x ˇ k h 2 σ X k − + σ R k σ X k + = σ R k σ X k − h 2 σ X k − + σ R k (19) \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}} x^k=h2σXk−+σRkhσXk−yk+σRkxˇk σXk+=h2σXk−+σRkσRkσXk−(19)根据上面的式子可以很直观的知道，其是一个递归推导，若已知 x ^ 0 \hat x_0 x^0 与 σ X 0 + \sigma^+_{X_{0}} σX0+，以及各个时刻的观测 y k y_k yk，则可递归推导：
【 x ^ 0 ， σ X 0 + ， y 1 】 → 【 x ^ 1 ， σ X 1 + , y 2 】 → ⋯ → 【 x ^ k ， σ X k + 】 (20) \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}}】 【x^0，σX0+，y1】→【x^1，σX1+,y2】→⋯→【x^k，σXk+】(20)对于(19) 式分别化简如下： x ^ k = h σ X k − y k + σ R k x ˇ k h 2 σ X k − + σ R k = h σ X k − h 2 σ X k − + σ R k y k + σ R k x ˇ k h 2 σ X k − + σ R k = h σ X k − h 2 σ X k − + σ R k y k + x ˇ ( σ R k + h 2 σ X k − ) − x ˇ k h 2 σ X k − h 2 σ X k − + σ R k = h σ X k − h 2 σ X k − + σ R k y k + x ˇ − x ˇ h 2 σ X k − h 2 σ X k − + σ R k = h σ X k − h 2 σ X k − + σ R k ( y k − h x ˇ ) + x ˇ (21) \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} x^k=h2σXk−+σRkhσXk−yk+σRkxˇk=h2σXk−+σRkhσXk−yk+h2σXk−+σRkσRkxˇk=h2σXk−+σRkhσXk−yk+h2σXk−+σRkxˇ(σRk+h2σXk−)−xˇkh2σXk−=h2σXk−+σRkhσXk−yk+xˇ−h2σXk−+σRkxˇh2σXk−=h2σXk−+σRkhσXk−(yk−hxˇ)+xˇ(21) σ X k + = σ R k σ X k − h 2 σ X k − + σ R k = ( h 2 σ X k − + σ R k ) σ X k − − h 2 σ X k − σ X k − h 2 σ X k − + σ R k = σ X k − − h 2 σ X k − σ X k − h 2 σ X k − + σ R k = ( 1 − h σ X k − h 2 σ X k − + σ R k h ) σ X k − (22) \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} σXk+=h2σXk−+σRkσRkσXk−=h2σXk−+σRk(h2σXk−+σRk)σXk−−h2σXk−σXk−=σXk−−h2σXk−+σRkh2σXk−σXk−=(1−h2σXk−+σRkhσXk−h)σXk−(22)可以看到，其都存在共同的一项，那就是卡尔曼增益项： k k = h σ X k − h 2 σ X k − + σ R k (23) \color{Green} \tag{23}k_k=\frac{h \sigma_{X_k}^{-} }{h^{2} \sigma_{X_k}^{-} +\sigma_{R_k}} kk=h2σXk−+σRkhσXk−(23)把其带如何到 (22) (23) 式可得： x ^ k = k k ( y k − h x ˇ ) + x ˇ σ X k + = ( 1 − h k k ) σ X k − (24) \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}^{-} x^k=kk(yk−hxˇ)+xˇ σXk+=(1−hkk)σXk−(24)
四、总结
通过上面一系列的推导，可以得到以下五个公式，其就是卡尔曼的五大核心公式，即 (15) 式中的两个公式(需展开)，以及 (23) (24)，整理如下：
①： x ˇ k = f x ^ k − 1 ②： σ X k − = f 2 σ X k − 1 + + σ Q k − 1 (25) \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}} ①：xˇk=fx^k−1 ②：σXk−=f2σXk−1++σQk−1(25) ③： k k = h σ X k − h 2 σ X k − + σ R k (26) \color{red} \tag{26}③：k_k=\frac{h \sigma_{X_k}^{-} }{h^{2} \sigma_{X_k}^{-} +\sigma_{R_k}} ③：kk=h2σXk−+σRkhσXk−(26) ④： x ^ k = k k ( y k − h x ˇ ) + x ˇ ⑤： σ X k + = ( 1 − h k k ) σ X k − (27) \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}^{-} ④：x^k=kk(yk−hxˇ)+xˇ ⑤：σXk+=(1−hkk)σXk−(27)
上面的五个式子很明显是递推的若假设已知 x ^ 0 \hat x_0 x^0、 σ X 0 + \sigma_{X_{0}}^+ σX0+、以及各个时刻观测 y k y_k yk，则可推导出出 x ^ k \hat x_k x^k、 σ X k + \sigma_{X_{k}}^+ σXk+，如下：
【 x ^ 0 , σ X 0 + , y 1 】 → 【 x ^ 1 , σ X 1 + , y 2 】 → ⋯ → 【 x ^ k , σ X k + 】 (28) \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}}^+】 【x^0,σX0+,y1】→【x^1,σX1+,y2】→⋯→【x^k,σXk+】(28)
不过上面推导的公式存在一个很大的问题，那就是其基于一维推导，实际应用过程中，很少使用到一维的卡尔曼滤波，虽然以这种方式进行推导，可以很好的理解整个推导过程，但是局限性太大。
另外，还有很多地方没有讲解清楚，如两个高斯分布的乘积与积分，高斯分布线性变化等，都未进行详细的讲解。不用着急，后续博客都会展开进行详细分析与推导。