讲解关于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)独家私有,禁止转载与抄袭,首次举报有谢!
一、前言
通过前面几篇博客分析，只能说对于概率论、数理统计、贝叶斯有了一定了解，但是对于贝叶斯滤波来说，还是存在好些不同的点，这里先把上节的内容回顾一下：
先验概率密度函数： f X ( x ) = X ∼ N ( μ 1 , σ 1 2 ) = 1 σ 1 2 π e − ( x − μ 1 ) 2 2 σ 1 2 (01) \color{Green} \tag{01} 先验概率密度函数：~f_X(x)=X \sim N\left(\mu_1, \sigma_1^{2}\right)=\frac{1}{ \sigma_1 \sqrt{2 \pi}} e^{-\frac{(x-\mu_1)^{2}}{2 \sigma_1^{2}}} 先验概率密度函数： fX(x)=X∼N(μ1,σ12)=σ12π1e−2σ12(x−μ1)2(01) 似然概率密度函数： f Y ∣ X ( y ∣ x ) = Y ∼ N ( μ 2 , σ 2 2 ) = 1 σ 2 2 π e − ( x − μ 2 ) 2 2 σ 2 2 (02) \color{Green} \tag{02} 似然概率密度函数：~f_{Y|X}(y|x)=Y \sim N\left(\mu_2, \sigma_2^{2}\right)=\frac{1}{ \sigma_2\sqrt{2 \pi}} e^{-\frac{(x-\mu_2)^{2}}{2 \sigma_2^{2}}} 似然概率密度函数： fY∣X(y∣x)=Y∼N(μ2,σ22)=σ22π1e−2σ22(x−μ2)2(02) f X ∣ Y ( x ∣ y ) = f X , Y ( x , y ) f Y ( y ) = f Y ∣ X ( y ∣ x ) f X ( x ) ∫ − ∞ + ∞ f Y ∣ X ( y ∣ x ) f X ( x ) d x = η f Y ∣ X ( y ∣ x ) f X ( x ) (03) \color{Green} \tag{03} f_{X \mid Y}(x \mid y)=\frac{f_{X, Y}(x, y)}{f_{Y}(y)}=\frac{f_{Y \mid X}(y \mid x) f_{X}(x)}{\int_{-\infty}^{+\infty} f_{Y \mid X}(y \mid x) f_{X}(x) \mathrm{d} x}=\eta f_{Y \mid X}(y \mid x) f_{X}(x) fX∣Y(x∣y)=fY(y)fX,Y(x,y)=∫−∞+∞fY∣X(y∣x)fX(x)dxfY∣X(y∣x)fX(x)=ηfY∣X(y∣x)fX(x)(03) 后验概率密度函数 : f X ∣ Y ( x ∣ y ) = N ( σ 1 2 σ 1 2 + σ 2 2 μ 2 + σ 2 2 σ 1 2 + σ 2 2 μ 1 ， σ 1 2 σ 2 2 σ 1 2 + σ 2 2 ) (04) \color{Green} \tag{04} 后验概率密度函数:~f_{X|Y}(x|y)=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) 后验概率密度函数: fX∣Y(x∣y)=N(σ12+σ22σ12μ2+σ12+σ22σ22μ1，σ12+σ22σ12σ22)(04)其上就是上篇博客推导出来的核心公式，但是这里注意，这里是基于正太分布的贝叶斯公式，而不是初始贝叶斯公式。也就是说，其已经部分实例化了贝叶斯公式，也就是认为先验、似然概率都符合正太分布，基于这个前提进行推导，当然大多数情况下，也确实都符合正太分布。
这里思考一个问题，如果我们使用传感器(力度记、轮速记、加速度记) 等等，通常都是可以连续测量的。那么怎么把这些数据利用起来呢？这里举个简单的例子，现在有一个传感器，你拿在手上，他能够测量你前进的速度(不考虑方向,就是一个标量速度)， t 0 t_{0} t0、 t 1 t_{1} t1、 t 2 t_{2} t2 . . . . . . ...... ...... t k t_{k} tk 时刻的速度 v 0 z v_{0}^z v0z、 v 1 z v_{1}^z v1z、 v 2 z v_{2}^z v2z . . . . . . ...... ...... v k z v_{k}^z vkz 都能测量出来，那么请问你在 t 0 t_{0} t0、 t 1 t_{1} t1、 t 2 t_{2} t2 . . . . . . ...... ...... t k t_{k} tk 各个时刻的速度是多少？有的朋友可能会说，不就是 v k z v_{k}^z vkz 吗？都测出来了，这还不简单？
这样的答案不是不行，但是太粗糙了，首先传感器本身就有误差的，测量结果本身就不是百分百准确，如 v k z v_{k}^z vkz 时刻的速度可能是 v k z − 0.001 v_{k}^z-0.001 vkz−0.001，或者 v k z + 0.001 v_{k}^z+0.001 vkz+0.001。都是有可能的，虽然我们不知道绝对真实值是多少，但是通常来说不可能和传感器车辆的结果一摸一样。除了传感器本身设计的误差，也还有其他环境的影响，比如磁场，温度等引起的误差。
细心的朋友应该都发现 v k z v_{k}^z vkz 的右上标有个 z z z，这里表示观测(测量)的意思，通常表示传感器测量结果，后续我们使用 v k v_k vk 表示真实值，但是 注意 \color{red} 注意 注意: 真实值是没有办法获取到的，我们的目的，是估算一个值 v ^ \hat v v^，让其逼近真实值，注意后续 “^” 这个帽子，就表示估算的意思。
二、场景思考
根据前面的示例，可以知道生活中右很多这种与观测(测量) 相关的事件，比如股票也是，为什什么？首先我们认为股票的涨跌跟前一天是有关系的，如使用 x k − 1 x_{k-1} xk−1 来表示第 k-1 天的涨跌度，如 x k − 1 = + 1 x_{k-1}=+1 xk−1=+1 表示当天上涨了一个点，建立一个这样的递推关系 x k = f ( x k − 1 ) x_k=f(x_{k-1}) xk=f(xk−1)，这个 f ( x k − 1 ) f(x_{k-1}) f(xk−1) 是广义的，完全可以根据自己的想法或需求定义得十分复杂。但是很明显 f ( x k − 1 ) f(x_{k-1}) f(xk−1) 这样的模型太简单了，单凭前一天的涨跌来判断第二天也比较潦草不靠谱，我们需要考虑更多的因素，比如利好消息数量 g k − 1 g_{k-1} gk−1，国际贸易趋势 m k − 1 m_{k-1} mk−1 等等，也就是类似于一个这样的表达式才是相对最靠谱的 x k = f ( x k − 1 , g k − 1 , m k − 1 , . . . ) x_k=f(x_{k-1},g_{k-1},m_{k-1},...) xk=f(xk−1,gk−1,mk−1,...)，不过这样写起来太复杂了，后续我们把额外的输入都统一记录为 u k u_k uk，免得后续推理显得过于臃肿，总的来说，建模成 x k = f ( x k − 1 , v k − 1 ) x_{k}=f(x_{k-1},v_{k-1}) xk=f(xk−1,vk−1) 才是合理的。
那么下面的，问题来了，首先 x k = f ( x k − 1 , v k − 1 ) x_{k}=f(x_{k-1},v_{k-1}) xk=f(xk−1,vk−1) 的涨跌程度是人为建模出来的，其必然纯在误差，如果没有误差就无敌了，直接首富就是你的了。所以添加一个噪声，得 x k = f ( x k − 1 , v k − 1 ) + q k x_{k}=f(x_{k-1},v_{k-1})+q_{k} xk=f(xk−1,vk−1)+qk，这里的 q q q 就表示噪声(通常假设符合高斯分布)，注意是及噪声的下标是 k k k 而不是 k − 1 k-1 k−1，因为是估算出来的结果有噪声。后续的目的就是要降低这个噪声，这个噪声越低，则表示我们预测的结果越准。这时，观测结果就派上用场了，这里记第 k k k 天的收盘涨跌度为 y k y_{k} yk，那么可以再建立一个模型，也就是 y k = h ( x k ) + r k y_k=h(x_{k})+r_k yk=h(xk)+rk，这里的 r r r 同样也表示噪声，那么我们现在就有两个模型了，也就是预测方程与测量方程，如下：
x k = f ( x k − 1 , v k − 1 ) + q k (05) \color{Green} \tag{05} x_{k}=f(x_{k-1},v_{k-1})+q_k xk=f(xk−1,vk−1)+qk(05) y k = h ( x k ) + r k (06) \color{Green} \tag{06} y_k=h(x_{k})+r_k yk=h(xk)+rk(06)其中 q、r 都表示噪声，实际应用过程中，都认为其时符合高斯分布的。为了形象的描述上面的过程请看如下图示：
上述的过程，就是 x k = f ( x k − 1 , v k − 1 ) x_{k}=f(x_{k-1},v_{k-1}) xk=f(xk−1,vk−1) 进行预测，然后通过观测 y k y_{k} yk 进行修正，修修之后预测的更加准。不过上述过程没有体现出额外信息 v k v_k vk 与 修正 这两个细节，再来看下图
该图基本描述了贝叶斯滤波的过程，且把噪声 q k , r k q_k,r_k qk,rk 都考虑在里面了。可以发现 x x x 有上标多了一个 − - − 或者 + + + 号。这里，单纯通过状态方程 f ( x k − 1 , v k − 1 ) + q k f(x_{k-1},v_{k-1}) + q_{k} f(xk−1,vk−1)+qk 计算的结果，记录为 x k − x_{k}^- xk−，其表示未利用观察数据进行修正，经过观测数据 y k y_k yk 修正之后，我们记录为 x k + x_{k}^+ xk+。
通过上面的图示与讲解，对于滤波的思想，大家可能十分理解了，但是怎么与贝叶斯联系起来的，文首的公式(01)、(02)、(03) 、(04) 似乎与上面的分析似乎没有半毛钱关系啊。其实不然，随着后面的分析，大家会明白他们联系。
三、核心定义(术语)
1.状态方程
上面讲解的过程中，都是使用小写的字母的，根据 P ( X ∣ Y k ) P(X|Y_k) P(X∣Yk)，根据 卡尔曼家族从零解剖-(01)预备知识点 中符号约定，可知其都表示随机变量的具体取值了，且为了后续推导的方便，暂不对 v k − 1 v_{k-1} vk−1 进行考虑，也就是说状态方随机变量表示与具体数值表示状态方程分别如下：
X k = f ( X k − 1 ) + Q k x x = f ( x k − 1 ) + q k (07) \color{Green} \tag{07} X_k=f(X_{k-1})+Q_{k}\\x_x=f(x_{k-1})+q_{k} Xk=f(Xk−1)+Qkxx=f(xk−1)+qk(07) 其中 x k x_k xk 表示随机变量 X k X_k Xk 的具体取值。 q k q_{k} qk 表示随机变量 Q k Q_{k} Qk 的具体取值，也就是说 q k q_{k} qk 是符合 Q k Q_{k} Qk的分布的。
2.测量方程
与上面类似，测量方程也使用大写字母表示随机变量，小写字母表示随机变量具体取值，其中 r k r_k rk 符合 随机变量 Q k Q_k Qk 分布。
Y k = h ( X k ) + R k y k = h ( x k ) + r k (08) \color{Green} \tag{08} Y_k=h(X_k)+R_k\\y_k=h(x_k)+r_k Yk=h(Xk)+Rkyk=h(xk)+rk(08)
3.额外说道
很多时候有的朋友会把预测方程与状态方程混合起来，认为他们时一致的没有如何区别，只是叫法不一样而已，个人觉得他们还是有一些细微区别的(纯个人看法)。与状态方程比较密切的还有一个名词叫做状态转移，实际上就是进行递推，也就是由上一状态，递推(转移)到下以状态，其核心在于公式的建模，如 x k = f ( x k − 1 ) + q k x_{k}=f(x_{k-1})+q_k xk=f(xk−1)+qk既然时建模，那就约精确越好，这样越能反应真实情况。但是预测方程，个人感觉时不一样的，所以的预测方程应该是不考虑噪声的，如下： x ^ k = f ( x k − 1 ) \hat x_{k}=f(x_{k-1}) x^k=f(xk−1)为什么不考虑噪声呢? 如果你都知道噪声了直接去掉不就可以吗？如果是高斯分布的，那么在噪声为 0 处取得最大值，可以忽略掉，所以预测方程和状态方程他们之间只差一个噪声，通常使用 “^” 符号表示预测。
四、贝叶斯推导条件
通过前面的分析，基本理解了贝叶斯滤波的核心思想，接下来就是要把所谓的状态方程、观测方程、贝叶斯公式都串联起来。为了方便推导，可以发现前面的公式都是使用小写字母且未加粗，即没有使用到向量与矩阵，向量与矩阵的推导后续我们再进行讲解。
( 1 ) : \color{blue}(1): (1): 首先已知 t 0 t_0 t0 时刻的状态，为 x 0 x_0 x0，设 t k t_k tk 时刻状态为 x k x_k xk，状态转移方程为 x k − = f ( x k − 1 ) + q k x_k^-=f(x_{k-1})+q_{k} xk−=f(xk−1)+qk，为了方便后续的推导，这里我们暂时先忽略掉 v k − 1 v_{k-1} vk−1，可以理解为没有额外的信息输入。其中 q k ∈ Q k ∼ N ( μ q k , σ q k 2 ) q_{k} \in Q_{k} \sim N(\mu_{q_{k}},\sigma_{q_{k}}^2) qk∈Qk∼N(μqk,σqk2)，这里的 μ q 0 , σ q 0 \mu_{q0},\sigma_{q0} μq0,σq0 初始值通常为已知量( k = 1 k=1 k=1时)，由建模者自行估算: X k = f ( X k − 1 ) + Q k x x = f ( x k − 1 ) + q k (09) \color{Green} \tag{09} X_k=f(X_{k-1})+Q_{k}\\x_x=f(x_{k-1})+q_{k} Xk=f(Xk−1)+Qkxx=f(xk−1)+qk(09)上式 x k x_k xk 表示随机变量 X k X_k Xk 的具体取值。 q k q_{k} qk 表示随机变量 Q k Q_{k} Qk 的具体取值，也就是说 q k q_{k} qk 是符合 Q k Q_{k} Qk的分布的。
( 2 ) : \color{blue}(2): (2): 已知 t k t_k tk 时刻的观测随机变量 Y k Y_k Yk 的具体结果 y k y_k yk，测量方程为 y k = h ( x k ) + r k y_k=h(x_{k})+r_k yk=h(xk)+rk，其中 r k ∈ R k ∼ N ( μ r k , σ r k 2 ) r_k \in R_k \sim N(\mu_{rk}^{},\sigma_{rk}^2) rk∈Rk∼N(μrk,σrk2)。这里的 μ r 1 , σ r 1 \mu_{r1}^{},\sigma_{r1} μr1,σr1 通常为已知量，由建模者自行设定，比如传感器的精度，可以由厂家提供，或者自己进行实验测试获得。测量方程如下：
Y k = h ( X k ) + R k y k = h ( x k ) + r k (10) \color{Green} \tag{10} Y_k=h(X_k)+R_k\\y_k=h(x_k)+r_k Yk=h(Xk)+Rkyk=h(xk)+rk(10) ( 3 ) : \color{blue}(3): (3): 已知条件与符号(若发现与前面定义存在冲突，都下述条件为主)
①各时刻的状态量随机变量: X 0 X_0 X0、 X 1 X_1 X1、 X 2 X_2 X2、 . . . . . . ...... ......、 X k X_{k} Xk。
②各时刻的观测量随机变量: Y 0 Y_0 Y0、 Y 1 Y_1 Y1、 Y 2 Y_2 Y2、 . . . . . . ...... ......、 Y k Y_{k} Yk
③各时刻的观测值: y 0 y_0 y0、 y 1 y_1 y1、 y 2 y_2 y2、 . . . . . . ...... ......、 y k y_{k} yk
④各时刻的状态随机噪声随机变量： Q 1 Q_1 Q1、 Q 2 Q_2 Q2、 Q 2 Q_2 Q2、 . . . . . . ...... ......、 Q k Q_{k} Qk
⑤各时刻的观测噪声随机变量: R 1 R_1 R1、 R 2 R_2 R2、 R 2 R_2 R2、 . . . . . . ...... ......、 R k R_{k} Rk
⑥各时刻的过程噪声随机变量概率密度函数： f Q 1 ( x ) f_{Q_1}(x) fQ1(x)、 f Q 2 ( x ) f_{Q_2}(x) fQ2(x)、 f Q 3 ( x ) f_{Q_3}(x) fQ3(x)、 . . . . . . ...... ......、 f Q k ( x ) f_{Q_k}(x) fQk(x)
⑦各时刻的观测噪声随机变量概率密度函数： f Q 1 ( x ) f_{Q_1}(x) fQ1(x)、 f Q 2 ( x ) f_{Q_2}(x) fQ2(x)、 f Q 3 ( x ) f_{Q_3}(x) fQ3(x)、 . . . . . . ...... ......、⑦ f Q k ( x ) f_{Q_k}(x) fQk(x)
⑧各时刻的状态量随机变量先验概率密度函数 f X 0 − f^-_{X_0} fX0−、 f X 1 − f^-_{X_1} fX1−、 f X 2 − f^-_{X_2} fX2−、 . . . . . . ...... ......、 f X k − f^-_{X_k} fXk−
⑨各时刻的状态量随机变量后验概率密度函数: f X 0 + f^+_{X_0} fX0+、 f X 1 + f^+_{X_1} fX1+、 f X 2 + f^+_{X_2} fX2+、 . . . . . . ...... ......、 f X k + f^+_{X_k} fXk+
⑩各时刻状态量随机变量与观测量随机变量的似然概率密度函数: f Y 1 ∣ X 1 ( y 1 ∣ x ) f_{Y_1|X_1}(y_1|x) fY1∣X1(y1∣x)、 f Y 2 ∣ X 2 ( y 2 ∣ x ) f_{Y_2|X_2}(y_2|x) fY2∣X2(y2∣x)、 . . . . . . ...... ......、 f Y k ∣ X k ( y k ∣ x ) f_{Y_k|X_k}(y_k|x) fYk∣Xk(yk∣x)
( 4 ) : \color{blue}(4): (4): 除了上述基本条件之外，条件概率中还有一个比较重要的性质需要记录一下，那就是 条件概率 条件概率 条件概率 里的条件 里的条件 里的条件 可以作逻 可以作逻 可以作逻 辑推导 辑推导 辑推导，这里列举一个示例：
P ( X = 5 ∣ Y = 1 , Z = 3 ) = P ( X + Z = 5 + 3 ∣ Y = 1 , Z = 3 ) = P ( X = 5 ∣ Y + Z = 1 + 3 ∣ Z = 3 ) (11) \color{Green} \tag{11} P(X=5|Y=1,Z=3)=P(X+Z=5+3|Y=1,Z=3)=P(X=5 | Y+Z=1+3|Z=3) P(X=5∣Y=1,Z=3)=P(X+Z=5+3∣Y=1,Z=3)=P(X=5∣Y+Z=1+3∣Z=3)(11)其实这是显而易见的，因为条件相当于是已知的，既然是已知的，那么当然可以用作逻辑推导，无论在符号 | 左边还是右边都可以。但是需要注意，这是在条件保持不变的情况下，如下就是一个反例： P ( X = 5 ∣ Y = 1 , Z = 3 ) ! = P ( X + Y = 5 + 1 ∣ Y = 1 ) P(X=5|Y=1,Z=3) != P(X+Y=5+1|Y=1) P(X=5∣Y=1,Z=3)!=P(X+Y=5+1∣Y=1)，因为这里的条件改变了，没有 Z = 1 Z=1 Z=1 这个条件被去除了，也就是说左右两边的基本条件都不一致，那么显然等式不一定成立了。
注意 : \color{red}注意: 注意: 首先要明白一点，我们想求得的结果是概率密度函数 f X k ∣ Y k ∣ ( x ∣ y k ) f_{X_k|Y_k|}(x|y_k) fXk∣Yk∣(x∣yk) 对应概率分布的期望。该概率密度函数对应的概率分布就是 P ( X ∣ Y k ) P(X|Y_k) P(X∣Yk)，根据 卡尔曼家族从零解剖-(01)预备知识点 中 (11) 式，可知：
P ( X ∣ Y k ) = P ( Y k ∣ X ) P ( X ) P ( Y k ) = η P ( Y k ∣ X ) P ( X ) (12) \color{Green} \tag{12} P\left(X \mid Y_k \right)=\frac{P\left(Y_k \mid X\right) P\left(X\right)}{P(Y_k)}=\eta P\left(Y_k \mid X\right) P\left(X\right) P(X∣Yk)=P(Yk)P(Yk∣X)P(X)=ηP(Yk∣X)P(X)(12) 也就是说，后续推导过程的核心就是求解先验概率分布 P ( Y k ∣ X ) P\left(Y_k \mid X\right) P(Yk∣X) 与 后验概率分布 P ( Y k ) P ( X ) P\left(Y_k\right) P\left(X\right) P(Yk)P(X)，再结合 η \eta η 即可求得后验概率分布 P ( X ∣ Y k ) P\left(X \mid Y_k \right) P(X∣Yk)。
五、公式推导
除了上述基本条件之外，还需要做一些额外的假设，这样我们的推导才能进行下去(能够简化计算)。
( 01 ) : \color{blue}(01): (01): 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 相互独立。
( 02 ) : \color{blue}(02): (02): 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 相互独立。
( 03 ) : \color{blue}(03): (03): X k − 1 X_{k-1} Xk−1 与 Q k Q_{k} Qk 独立， X k X_{k} Xk 与 R k R_k Rk 独立 (可由前两个假设推导出来，后续具体讲解)。
1.先验(预测)推导
已知 k k k 时刻随机状态量 X k X_k Xk(未知) 后验概率密度函数 f X k + ( x ) f_{X_k}^+(x) fXk+(x)，状态转移函数 f ( x ) f(x) f(x)，时刻过程噪声随机变量 Q k Q_k Qk 对应的概率密度函数 f Q k ( x ) f_{Q_k}(x) fQk(x)，求解 k k k 时状态随机变量 X k X_k Xk 对应先验概率密度函数 f X k − ( x ) f^-_{X_k}(x) fXk−(x)，因为已知概率密度分布，通过积分即可求得其对应得概率分布 F X k − F^-_{X_k} FXk−。推导过程如下, 首先：
F X K − = P ( X k ≤ x ) = ∑ u = − ∞ + ∞ P ( X k = u ) (13) \color{Green} \tag{13} F_{X_K}^-=P(X_k\leq x)=\sum_{u=-\infty}^{+\infty}P(X_k=u) FXK−=P(Xk≤x)=u=−∞∑+∞P(Xk=u)(13)上式利用到了连续化离散，无穷小量化累加的形式，核心思想就是面积累加。根据前面的递推关系，可以知道 x k = f ( x k − 1 ) x_k=f(x_{k-1}) xk=f(xk−1)，可以把 x k − 1 x_{k-1} xk−1 看作 x k x_k xk的条件，根据全概率公式可以得到， P ( X k = u ) = ∑ v = − ∞ + ∞ P ( X k = u ∣ X k − 1 = v ) P ( X k − 1 = v ) P(X_k=u)=\sum_{v=-\infty}^{+\infty}P(X_k=u|X_{k-1}=v)P(X_{k-1}=v) P(Xk=u)=∑v=−∞+∞P(Xk=u∣Xk−1=v)P(Xk−1=v)。上式可得： = ∑ u = − ∞ + ∞ ∑ v = − ∞ + ∞ P ( X k = u ∣ X k − 1 = v ) P ( X k − 1 = v ) (14) \color{Green} \tag{14} =\sum_{u=-\infty}^{+\infty}\sum_{v=-\infty}^{+\infty}P(X_k=u~|~X_{k-1}=v)P(X_{k-1}=v) =u=−∞∑+∞v=−∞∑+∞P(Xk=u ∣ Xk−1=v)P(Xk−1=v)(14) 上面的式子需要注意一下，累加已经有了两个符号，即 u u u 与 v v v，千万不要混淆了。接着比较有意义的一步就要出现，这里需要利用到 (11) 式的 条件概率 \color{red} 条件概率 条件概率 里的条件 \color{red} 里的条件 里的条件 可以作逻 \color{red} 可以作逻 可以作逻 辑推导 \color{red} 辑推导 辑推导，上面的 X k − 1 = v X_{k-1}=v Xk−1=v 是一个条件，那么我们可以把这个条件利用起来，带入到状态方程 f ( x ) f(x) f(x)，进而得到：
= ∑ u = − ∞ + ∞ ∑ v = − ∞ + ∞ P [ X k − f ( X k − 1 ) = u − f ( v ) ∣ X k − 1 = v ] P ( X k − 1 = v ) (15) \color{Green} \tag{15} =\sum_{u=-\infty}^{+\infty} \sum_{v=-\infty}^{+\infty}P[X_k-f(X_{k-1})=u-f(v)~|~X_{k-1}=v]P(X_{k-1}=v) =u=−∞∑+∞v=−∞∑+∞P[Xk−f(Xk−1)=u−f(v) ∣ Xk−1=v]P(Xk−1=v)(15) 这个时候 核心关键 \color{red}核心关键 核心关键 关键就出现了，参考状态方程(09)式，移项变号，可知 X k − f ( X k − 1 ) = Q k X_k-f(X_{k-1})=Q_{k} Xk−f(Xk−1)=Qk， 把该等式带入到上式，可得：
= ∑ u = − ∞ + ∞ ∑ v = − ∞ + ∞ P [ Q k = u − f ( v ) ∣ X k − 1 = v ] P ( X k − 1 = v ) (16) \color{Green} \tag{16} =\sum_{u=-\infty}^{+\infty} \sum_{v=-\infty}^{+\infty}P[Q_{k}=u-f(v)~|~X_{k-1}=v]P(X_{k-1}=v) =u=−∞∑+∞v=−∞∑+∞P[Qk=u−f(v) ∣ Xk−1=v]P(Xk−1=v)(16)
到这里为止，需要使用到前面给定的条件 【 ( 03 ) : \color{blue}(03): (03): X k − 1 X_{k-1} Xk−1 与 Q k Q_{k} Qk 独立， X k X_{k} Xk 与 R k R_k Rk 独立】。既然 X k − 1 X_{k-1} Xk−1 与 Q k Q_k Qk 独立，那么上式的条件则可以直接省略了，化简如下：
= ∑ u = − ∞ + ∞ ∑ v = − ∞ + ∞ P [ Q k = u − f ( v ) ] P ( X k − 1 = v ) (17) \color{Green} \tag{17} =\sum_{u=-\infty}^{+\infty} \sum_{v=-\infty}^{+\infty}P[Q_{k}=u-f(v)]P(X_{k-1}=v) =u=−∞∑+∞v=−∞∑+∞P[Qk=u−f(v)]P(Xk−1=v)(17) 更进一步，需要把概率分布通过概率密度进行表示，注意 P ( X k − 1 = v ) P(X_{k-1}=v) P(Xk−1=v) 等价于 f X k − 1 + ( v ) ϵ f^+_{X_{k-1}}(v) \epsilon fXk−1+(v)ϵ，因为这里的 X k − 1 X_{k-1} Xk−1 其表示 k − 1 k-1 k−1 时刻后验概率，所以： = ∑ u = − ∞ + ∞ lim ϵ → 0 { ∑ v = − ∞ + ∞ f Q k [ u − f ( v ) ] ϵ ⋅ f X k − 1 + ( v ) ϵ } (18) \color{Green} \tag{18} = {\sum_{u=-\infty}^{+\infty} \lim _{\epsilon \rightarrow 0} \{\sum_{v=-\infty}^{+\infty} f_{Q_{k}}[u-f(v)] \epsilon ·f^+_{X_{k-1}}(v) \epsilon}\} =u=−∞∑+∞ϵ→0lim{
v=−∞∑+∞fQk[u−f(v)]ϵ⋅fXk−1+(v)ϵ}(18) 根据积分定义，上式等价于： = ∑ u = − ∞ + ∞ lim ϵ → 0 { ∫ − ∞ + ∞ f Q k [ u − f ( v ) ] f X k − 1 + ( v ) d v ⋅ ϵ } (19) \color{Green} \tag{19} = {\sum_{u=-\infty}^{+\infty} \lim _{\epsilon \rightarrow 0} \{ \int_{-\infty}^{+\infty} f_{Q_{k}}[u-f(v)] f^+_{X_{k-1}} (v) \mathrm{d} v· \epsilon}\} =u=−∞∑+∞ϵ→0lim{
∫−∞+∞fQk[u−f(v)]fXk−1+(v)dv⋅ϵ}(19)
注意上式子消掉的是左边的 ϵ \epsilon ϵ，因为消掉的 ∑ \sum ∑ 符号对应额变量是 v v v，现在因为$ \epsilon$ 趋向于无穷小，因为积分与积分变量无关，这里把其符号修改成 v v v，可得双重积分如下：
= ∫ − ∞ + ∞ ∫ − ∞ + ∞ f Q k [ u − f ( v ) ] f X k − 1 + ( v ) d v d u (20) \color{Green} \tag{20} = { \int_{-\infty}^{+\infty} \int_{-\infty}^{+\infty} f_{Q_{k}}[u-f(v)] f^+_{X_{k-1}}(v) \mathrm{d}v \mathrm{d} u } =∫−∞+∞∫−∞+∞fQk[u−f(v)]fXk−1+(v)dvdu(20) 这里再进一步把 u u u 换成 x x x 得：
= ∫ − ∞ + ∞ ∫ − ∞ + ∞ f Q k [ x − f ( v ) ] f X k − 1 + ( v ) d v d x (21) \color{Green} \tag{21} = { \int_{-\infty}^{+\infty} \int_{-\infty}^{+\infty} f_{Q_{k}}[x-f(v)] f^+_{X_{k-1}}(v) \mathrm{d}v \mathrm{d} x} =∫−∞+∞∫−∞+∞fQk[x−f(v)]fXk−1+(v)dvdx(21)
现在把前面的推导过程梳理一下：
F X k − ( x ) = P ( X k ≤ x ) = ∑ u = − ∞ x P ( X k = u ) ⇒ 化连续为离散无穷小的累加 = ∑ u = − ∞ x ∑ v = − ∞ + ∞ P ( X k = u ∣ X k − 1 = v ) P ( X k − 1 = v ) ⇒ 全概率公式 = ∑ u = − ∞ x ∑ v = − ∞ + ∞ P [ X k − f ( X k − 1 ) = u − f ( v ) ∣ X 0 k − 1 = v ] P ( X k − 1 = v ) ⇒ 条件概率里的条件可以作逻辑推导 = ∑ u = − ∞ x ∑ v = − ∞ + ∞ P [ Q k = u − f ( v ) ∣ X k − 1 = v ] P ( X k − 1 = v ) ⇒ 状态方程 = ∑ u = − ∞ x ∑ v = − ∞ + ∞ P [ Q k = u − f ( v ) ] P ( X k − 1 = v ) ⇒ X k − 1 与 Q k 相互独立 = ∑ u = − ∞ x { lim ϵ → 0 ∑ v = − ∞ + ∞ f Q k [ u − f ( v ) ] ⋅ ϵ ⋅ f X k − 1 + ( v ) ⋅ ϵ } ⇒ 类似二维连续型随机变 = ∑ u = − ∞ x { lim ϵ → 0 ∫ − ∞ + ∞ f Q k [ u − f ( v ) ] f X k − 1 † ( v ) d v ⋅ ϵ } ⇒ 积分定义 = ∫ − ∞ x ∫ − ∞ + ∞ f Q k [ u − f ( v ) ] f X k − 1 + ( v ) d v d u ⇒ 积分定义 = ∫ − ∞ x ∫ − ∞ + ∞ f Q k [ x − f ( v ) ] f X k − 1 + ( v ) d v d x ⇒ 替换自变量符号 u 为 x (22) \color{Green} \tag{22} \begin{aligned} F_{X_{k}}^{-}(x) & =P\left(X_{k} \leq x\right) \\ & =\sum_{u=-\infty}^{x} P\left(X_{k}=u\right) \Rightarrow \text { 化连续为离散无穷小的累加 } \\ & =\sum_{u=-\infty}^{x} \sum_{v=-\infty}^{+\infty} P\left(X_{k}=u \mid X_{k-1}=v\right) P\left(X_{k-1}=v\right) \Rightarrow \text { 全概率公式 } \\ & =\sum_{u=-\infty}^{x} \sum_{v=-\infty}^{+\infty} P\left[X_{k}-f\left(X_{k-1}\right)=u-f(v) \mid X_{0k-1}=v\right] P\left(X_{k-1}=v\right) \Rightarrow \text { 条件概率里的条件可以作逻辑推导 } \\ & =\sum_{u=-\infty}^{x} \sum_{v=-\infty}^{+\infty} P\left[Q_{k}=u-f(v) \mid X_{k-1}=v\right] P\left(X_{k-1}=v\right) \Rightarrow \text { 状态方程 } \\ & =\sum_{u=-\infty}^{x} \sum_{v=-\infty}^{+\infty} P\left[Q_{k}=u-f(v)\right] P\left(X_{k-1}=v\right) \Rightarrow X_{k-1} \text { 与 } Q_{k} \text { 相互独立 } \\ & =\sum_{u=-\infty}^{x}\left\{\lim _{\epsilon \rightarrow 0} \sum_{v=-\infty}^{+\infty} f_{Q_{k}}[u-f(v)] \cdot \epsilon \cdot f_{X_{k-1}}^{+}(v) \cdot \epsilon\right\} \Rightarrow \text { 类似二维连续型随机变 } \\ & =\sum_{u=-\infty}^{x}\left\{\lim _{\epsilon \rightarrow 0} \int_{-\infty}^{+\infty} f_{Q_{k}}[u-f(v)] f_{X_{k-1}}^{\dagger}(v) \mathrm{d} v \cdot \epsilon\right\} \Rightarrow \text { 积分定义 } \\ & =\int_{-\infty}^{x} \int_{-\infty}^{+\infty} f_{Q_{k}}[u-f(v)] f_{X_{k-1}}^{+}(v) \mathrm{d} v \mathrm{~d} u \Rightarrow \text { 积分定义 } \\ & =\int_{-\infty}^{x} \int_{-\infty}^{+\infty} f_{Q_{k}}[x-f(v)] f_{X_{k-1}}^{+}(v) \mathrm{d} v ~\mathrm{d} x \Rightarrow \text { 替换自变量符号 } u \text { 为 } x \end{aligned} FXk−(x)=P(Xk≤x)=u=−∞∑xP(Xk=u)⇒ 化连续为离散无穷小的累加 =u=−∞∑xv=−∞∑+∞P(Xk=u∣Xk−1=v)P(Xk−1=v)⇒ 全概率公式 =u=−∞∑xv=−∞∑+∞P[Xk−f(Xk−1)=u−f(v)∣X0k−1=v]P(Xk−1=v)⇒ 条件概率里的条件可以作逻辑推导 =u=−∞∑xv=−∞∑+∞P[Qk=u−f(v)∣Xk−1=v]P(Xk−1=v)⇒ 状态方程 =u=−∞∑xv=−∞∑+∞P[Qk=u−f(v)]P(Xk−1=v)⇒Xk−1 与 Qk 相互独立 =u=−∞∑x{
ϵ→0limv=−∞∑+∞fQk[u−f(v)]⋅ϵ⋅fXk−1+(v)⋅ϵ}⇒ 类似二维连续型随机变 =u=−∞∑x{
ϵ→0lim∫−∞+∞fQk[u−f(v)]fXk−1†(v)dv⋅ϵ}⇒ 积分定义 =∫−∞x∫−∞+∞fQk[u−f(v)]fXk−1+(v)dv du⇒ 积分定义 =∫−∞x∫−∞+∞fQk[x−f(v)]fXk−1+(v)dv dx⇒ 替换自变量符号 u 为 x(22)通过前面一系列推导，可以得到如下结论：
F X k − ( x ) = ∫ − ∞ x f X k − 1 ( x ) d x = ∫ − ∞ x ∫ − ∞ + ∞ f Q k [ x − f ( v ) ] f X k − 1 + ( v ) d v d x (23) \color{Green} \tag{23} F_{X_{k}}^{-}(x)=\int_{-\infty}^{x}f_{X_{k-1}}(x)\mathrm{d}x=\int_{-\infty}^{x} \int_{-\infty}^{+\infty} f_{Q_{k}}[x-f(v)] f_{X_{k-1}}^{+}(v) \mathrm{d} v ~\mathrm{d} x FXk−(x)=∫−∞xfXk−1(x)dx=∫−∞x∫−∞+∞fQk[x−f(v)]fXk−1+(v)dv dx(23)认真观察先上式，抵消外面的对 x x x 的积分，可以得到下式子：
f X k − ( x ) = d F X 1 − ( x ) d x = ∫ − ∞ + ∞ f Q k [ x − f ( v ) ] f X k − 1 + ( v ) d v (24) \color{red} \tag{24} f_{X_{k}}^-(x)=\frac{\mathrm{d} F_{X_1}^-(x)}{\mathrm{d} x}= \int_{-\infty}^{+\infty} f_{Q_{k}}[x-f(v)] f_{X_{k-1}}^{+}(v) \mathrm{d} v fXk−(x)=dxdFX1−(x)=∫−∞+∞fQk[x−f(v)]fXk−1+(v)dv(24) 推导完毕。可以发现，先验概率密度函数本质来源于状态方程。
2.似然(测量)推导
已知 k k k 时刻观测量随机变量 Y k Y_k Yk 的取值 y 1 y_1 y1，求解 k k k 时刻状态量随机变量与观测量随机变量的似然概率密度函数 f Y k ∣ X k ( y 1 ∣ x ) f_{
{Y_k}|X_k}(y_1|x) fYk∣Xk(y1∣x)：
f Y k ∣ X k ( y k ∣ x ) = lim ϵ → 0 F Y k ∣ X k ( y k + ϵ ∣ x ) − F Y k ∣ X k ( y k ∣ x ) ϵ ⇒ 导数的定义 = lim ϵ → 0 P ( y k ≤ Y k ≤ y k + ϵ ∣ X k = x ) ϵ ⇒ 累积分布函数的性质 = lim ϵ → 0 P [ y k − h ( x ) ≤ Y k − h ( X k ) ≤ y k − h ( x ) + ϵ ∣ X k = x ] ϵ ⇒ 条件概率里的条件可以作逻辑推导 = lim ϵ → 0 P [ y k − h ( x ) ≤ R k ≤ y k − h ( x ) + ϵ ∣ X k = x ] ϵ ⇒ 观测方程 = lim ϵ → 0 P [ y k − h ( x ) ≤ R k ≤ y k − h ( x ) + ϵ ] ϵ ⇒ X k 与 R k 相互独立 = lim ϵ → 0 F R k [ y k − h ( x ) + ϵ ] − F R k [ y k − h ( x ) ] ϵ ⇒ 累积分布函数的性质 = f R k [ y k − h ( x ) ] ⇒ 导数的定义 (25) \color{Green} \tag{25} \begin{aligned} f_{Y_{k} \mid X_{k}}\left(y_{k} \mid x\right) & =\lim _{\epsilon \rightarrow 0} \frac{F_{Y_{k} \mid X_{k}}\left(y_{k}+\epsilon \mid x\right)-F_{Y_{k} \mid X_{k}}\left(y_{k} \mid x\right)}{\epsilon} \Rightarrow \text { 导数的定义 } \\ & =\lim _{\epsilon \rightarrow 0} \frac{P\left(y_{k} \leq Y_{k} \leq y_{k}+\epsilon \mid X_{k}=x\right)}{\epsilon} \Rightarrow \text { 累积分布函数的性质 } \\ & =\lim _{\epsilon \rightarrow 0} \frac{P\left[y_{k}-h(x) \leq Y_{k}-h\left(X_{k}\right) \leq y_{k}-h(x)+\epsilon \mid X_{k}=x\right]}{\epsilon} \Rightarrow 条件概率里的条件可以作逻辑推导\\ & =\lim _{\epsilon \rightarrow 0} \frac{P\left[y_{k}-h(x) \leq R_{k} \leq y_{k}-h(x)+\epsilon \mid X_{k}=x\right]}{\epsilon} \Rightarrow \text { 观测方程 } \\ & =\lim _{\epsilon \rightarrow 0} \frac{P\left[y_{k}-h(x) \leq R_{k} \leq y_{k}-h(x)+\epsilon\right]}{\epsilon} \Rightarrow X_{k} \text { 与 } R_{k} \text { 相互独立 } \\ & =\lim _{\epsilon \rightarrow 0} \frac{F_{R_{k}}\left[y_{k}-h(x)+\epsilon\right]-F_{R_{k}}\left[y_{k}-h(x)\right]}{\epsilon} \Rightarrow \text { 累积分布函数的性质 } \\ & =f_{R_{k}}\left[y_{k}-h(x)\right] \Rightarrow \text { 导数的定义 } \end{aligned} fYk∣Xk(yk∣x)=ϵ→0limϵFYk∣Xk(yk+ϵ∣x)−FYk∣Xk(yk∣x)⇒ 导数的定义 =ϵ→0limϵP(yk≤Yk≤yk+ϵ∣Xk=x)⇒ 累积分布函数的性质 =ϵ→0limϵP[yk−h(x)≤Yk−h(Xk)≤yk−h(x)+ϵ∣Xk=x]⇒条件概率里的条件可以作逻辑推导=ϵ→0limϵP[yk−h(x)≤Rk≤yk−h(x)+ϵ∣Xk=x]⇒ 观测方程 =ϵ→0limϵP[yk−h(x)≤Rk≤yk−h(x)+ϵ]⇒Xk 与 Rk 相互独立 =ϵ→0limϵFRk[yk−h(x)+ϵ]−FRk[yk−h(x)]⇒ 累积分布函数的性质 =fRk[yk−h(x)]⇒ 导数的定义 (25)这里就不像前面，一个一个等式分析讲解了，因为整体逻辑都差不多。最终可以得到：
f Y k ∣ X k ( y k ∣ x ) = f R k [ y k − h ( x ) ] (26) \color{red} \tag{26} f_{Y_{k} \mid X_{k}}(y_{k} \mid x)=f_{R_{k}}\left[y_{k}-h(x)\right] fYk∣Xk(yk∣x)=fRk[yk−h(x)](26)
可以发现，似然概率密度函数本质来源于观测方程。
3.后验概率(密度)
接着， k k k 时状态随机变量 X k X_k Xk 对应先验概率密度函数 f X k − ( x ) f^-_{X_k}(x) fXk−(x)，联合预测步得到的 k k k 时刻状态量随机变量 X k X_k Xk 的先验概率密度函数 f X k − ( x ) f_{X_k}^-(x) fXk−(x)，求解 k k k 时刻状态量随机变量 X k X_k Xk 的后验概率密度函数 f X k + ( x ) f_{X_k}^+(x) fXk+(x)，也就是联合 (24) 与 (26) ，再参看(03) 式，可得到：
f X k + ( x ) = η k ⋅ f X k ∣ Y k ( x ) ⋅ f X k − ( x ) = η k ⋅ f R k [ y k − h ( x ) ] ⋅ f X k − ( x ) (27) \color{red} \tag{27} 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]· f_{X_k}^-(x) fXk+(x)=ηk⋅fXk∣Yk(x)⋅fXk−(x)=ηk⋅fRk[yk−h(x)]⋅fXk−(x)(27) η k = [ ∫ − ∞ + ∞ f R k [ y k − h ( x ) ] ⋅ f X k − ( x ) d x ] − 1 (28) \color{Green} \tag{28} \eta_k=[\int_{-\infty}^{+\infty}f_{R_{k}}\left[y_{k}-h(x)\right]· f_{X_k}^-(x)\mathrm{d} x]^{-1} ηk=[∫−∞+∞fRk[yk−h(x)]⋅fXk−(x)dx]−1(28) f X k − ( x ) = d F X 1 − ( x ) d x = ∫ − ∞ + ∞ f Q k [ x − f ( v ) ] f X k − 1 + ( v ) d v (29) \ \color{Green} \tag{29} f_{X_{k}}^-(x)=\frac{\mathrm{d} F_{X_1}^-(x)}{\mathrm{d} x}= \int_{-\infty}^{+\infty} f_{Q_{k}}[x-f(v)] f_{X_{k-1}}^{+}(v) \mathrm{d} v fXk−(x)=dxdFX1−(x)=∫−∞+∞fQk[x−f(v)]fXk−1+(v)dv(29)
六、递推流程
前面提到，贝叶斯滤波与贝叶斯公式最大不通在于叶斯滤波是一种递推的形式，由预测步和更新步的推导结果，可得到由 k − 1 k-1 k−1 时刻状态量随机变量 X k − 1 X_{k-1} Xk−1 的后验概率密度函数 f X k − 1 + f_{X_{k-1}}^+ fXk−1+ 得到 k − 1 k-1 k−1 时刻状态量随机变量 X k X_{k} Xk 的后验概率密度 f X k + f_{X_{k}}^+ fXk+ 递推流程如下： f X 0 + ( x ) ⟹ 预测 f X 1 − ( x ) = ∫ − ∞ + ∞ f Q 1 [ x − f ( v ) ] f X 0 + ( v ) d v ⟹ 观测更新 f X 1 + ( x ) = η 1 ⋅ f R 1 [ y 1 − h ( x ) ] ⋅ f X 1 − ( x ) ⟹ 预测 f X 2 − ( x ) = ∫ − ∞ + ∞ f Q 2 [ x − f ( v ) ] f X 1 + ( v ) d v ⟹ 观测更新 f X 2 + ( x ) = η 2 ⋅ f R 2 [ y 2 − h ( x ) ] ⋅ f X 2 − ( x ) .. ⟹ 预测 f X k − ( x ) = ∫ − ∞ + ∞ f Q k [ x − f ( v ) ] f X k − 1 + ( v ) d v ⟹ 观测更新 f X k + ( x ) = η k ⋅ f R k [ y k − h ( x ) ] - f X k − ( x ) (28) \color{Green} \tag{28} \begin{array}{l} f_{X_{0}}^{+}(x) &\stackrel{\text { 预测 }}{\Longrightarrow} &f_{X_{1}}^{-}(x)=\int_{-\infty}^{+\infty} f_{Q_{1}}[x-f(v)] f_{X_{0}}^{+}(v) \mathrm{d} v &\stackrel{\text { 观测更新 }}{\Longrightarrow} &f_{X_{1}}^{+}(x)=\eta_{1} \cdot f_{R_{1}}\left[y_{1}-h(x)\right] \cdot f_{X_{1}}^{-}(x) \\ &\stackrel{\text { 预测 }}{\Longrightarrow} &f_{X_{2}}^{-}(x)=\int_{-\infty}^{+\infty} f_{Q_{2}}[x-f(v)] f_{X_{1}}^{+}(v) \mathrm{d} v& \stackrel{\text { 观测更新 }}{\Longrightarrow} &f_{X_{2}}^{+}(x)=\eta_{2} \cdot f_{R_{2}}\left[y_{2}-h(x)\right]\cdot f_{X_{2}}^{-}(x) \\ &\text {.. } \\ &\stackrel{\text { 预测 }}{\Longrightarrow} &f_{X_{k}}^{-}(x)=\int_{-\infty}^{+\infty} f_{Q_{k}}[x-f(v)] f_{X_{k-1}}^{+}(v) \mathrm{d} v&\stackrel{\text { 观测更新 }}{\Longrightarrow}& f_{X_{k}}^{+}(x)=\eta_{k} \cdot f_{R_{k}}\left[y_{k}-h(x)\right]\text { - } f_{X_{k}}^{-}(x) \\ \end{array} fX0+(x)⟹ 预测 ⟹ 预测 .. ⟹ 预测 fX1−(x)=∫−∞+∞fQ1[x−f(v)]fX0+(v)dvfX2−(x)=∫−∞+∞fQ2[x−f(v)]fX1+(v)dvfXk−(x)=∫−∞+∞fQk[x−f(v)]fXk−1+(v)dv⟹ 观测更新 ⟹ 观测更新 ⟹ 观测更新 fX1+(x)=η1⋅fR1[y1−h(x)]⋅fX1−(x)fX2+(x)=η2⋅fR2[y2−h(x)]⋅fX2−(x)fXk+(x)=ηk⋅fRk[yk−h(x)] - fXk−(x)(28)最总，可以得到 k 时刻状态量随机变量 X k {X_k} Xk 的最优估计 x k ^ \hat{x_k} xk^: x ^ k = E [ f X k + ( x ) ] = ∫ − ∞ + ∞ x f X k + ( x ) d x (29) \color{Green} \tag{29} \hat{x}_{k}=E\left[f_{X_{k}}^{+}(x)\right]=\int_{-\infty}^{+\infty} x f_{X_{k}}^{+}(x) \mathrm{d} x x^k=E[fXk+(x)]=∫−∞+∞xfXk+(x)dx(29)
七、应用流程
( 1 ) 设初值 : \color{blue}(1)设初值: (1)设初值: 首先要设定好状态随机变量 X 0 X_0 X0 的后验概率密度函数 f X 0 − ( x ) f_{X_0}^-(x) fX0−(x)。
( 2 ) 预测步 : \color{blue}(2)预测步: (2)预测步: 求得 k k k 时刻状态量随机变量 X k X_k Xk 的先验概率密度函数：
f X k − ( x ) = ∫ − ∞ + ∞ f Q k [ x − f ( v ) ] f X k − 1 + ( v ) d v (30) \color{Green} \tag{30} 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(30)
( 3 ) 预测步 : \color{blue}(3)预测步: (3)预测步: 求得 k k k 时刻状态量随机变量 X k X_k Xk 的后验概率密度函数： f X k + ( x ) = η k ⋅ f X k ∣ Y k ( x ) ⋅ f X k − ( x ) = η k ⋅ f R k [ y k − h ( x ) ] ⋅ f X k − ( x ) (31) \color{Green} \tag{31} 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]· f_{X_k}^-(x) fXk+(x)=ηk⋅fXk∣Yk(x)⋅fXk−(x)=ηk⋅fRk[yk−h(x)]⋅fXk−(x)(31) η k = [ ∫ − ∞ + ∞ f R k [ y k − h ( x ) ] ⋅ f X k − ( x ) d x ] − 1 (32) \color{Green} \tag{32} \eta_k=[\int_{-\infty}^{+\infty}f_{R_{k}}\left[y_{k}-h(x)\right]· f_{X_k}^-(x)\mathrm{d} x]^{-1} ηk=[∫−∞+∞fRk[yk−h(x)]⋅fXk−(x)dx]−1(32)
( 4 ) 预测步 : \color{blue}(4)预测步: (4)预测步: 最终联合上2两式，求得 k k k 时刻状态量随机变量 X k X_k Xk 后验估计 x ^ k \hat{x}_{k} x^k: x ^ k = E [ f X k + ( x ) ] = ∫ − ∞ + ∞ x f X k + ( x ) d x (33) \color{Green} \tag{33} \hat{x}_{k}=E\left[f_{X_{k}}^{+}(x)\right]=\int_{-\infty}^{+\infty} x f_{X_{k}}^{+}(x) \mathrm{d} x x^k=E[fXk+(x)]=∫−∞+∞xfXk+(x)dx(33)
八、结语
前面 【五、公式推导】 有提到三个假设:
( 01 ) : \color{blue}(01): (01): X 0 X_0 X0 与 Q 2 Q_2 Q2、 Q 2 Q_2 Q2、 Q 3 Q_3 Q3、 . . . . . . ...... ......、 Q k Q_{k} Qk 相互独立。
( 02 ) : \color{blue}(02): (02): 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 相互独立。
( 03 ) : \color{blue}(03): (03): X k − 1 X_{k-1} Xk−1 与 Q k Q_{k} Qk 独立， X k X_{k} Xk 与 R k R_k Rk 独立
注意 : \color{red}注意: 注意: 上面的假设十分重要，比如，没有这个前提假设，则推导过程中的 (17) 式子无法成立。前两个假设比较好理解，主观判断即可。如下用 F F F 广义表示变量之间的关系：
X k = F ( X k − 1 , Q k , R k ) (34) \color{Green} \tag{34} X_k=F(X_{k-1},Q_{k},R_{k}) Xk=F(Xk−1,Qk,Rk)(34)由于 X k X_k Xk 与 Q k Q_{k} Qk、 R k R_{k} Rk 独立(已设)，所以只需要证明 X k X_k Xk 与 X k − 1 X_{k-1} Xk−1 独立即可，又：
X k − 1 = F ( X k − 2 , Q k − 1 , R k − 1 ) (35) \color{Green} \tag{35} X_{k-1}=F(X_{k-2},Q_{k-1},R_{k-1}) Xk−1=F(Xk−2,Qk−1,Rk−1)(35)把上式带入到 (34)，那么现在就比变成 X k X_k Xk 与 X k − 2 X_{k-2} Xk−2、 Q k − 1 Q_{k-1} Qk−1 、 R k − 1 R_{k-1} Rk−1 相互独立，一直展开下去，直到 X 0 X_0 X0，实际上可得如下广义表达式：
X k = F ( X 0 , Q 1 , . . . , Q k , R 1 , . . . , R k ) (36) \color{Green} \tag{36} X_{k}=F(X_0,Q_1,...,Q_{k},R_1,...,R_k) Xk=F(X0,Q1,...,Qk,R1,...,Rk)(36)由于 X 0 X_0 X0 与他们都独立，故结果 X k X_k Xk 与他们同样独立，得证。到这为止，对 贝叶斯滤波 可以说有一定程度了解了，后续我们在其基础上，对卡尔曼滤波进行推导。