贝叶斯滤波

1. 概率基础回顾

条件概率： $$\begin{gathered} p(x|y)=p(x,y)/p(y) \\ p(x,y)=p(x|y)p(y)=p(y|x)p(x) \end{gathered}$$ 全概率公式： $$p(x)=\sum _{y}p(x,y)=\sum _{y}p(x|y)p(y)$$ 全概率公式的意义在于，当某一事件的概率难以求得时，可转化为在一系列条件下发生概率的和。

2. 贝叶斯公式

2.1 贝叶斯公式

基于条件概率公式和全概率公式，我们可以导出贝叶斯公式： $$ \begin{array}{c} P(x,y) = P(x|y)P(y) = P(y|x)P(x)\ \Rightarrow \

\end{array} $$

$$P(x|y)=\frac{P(y|x)P(x)}{P(y)}=\frac{P(y|x)P(x)}{\sum _{y}p(x|y)p(y)}$$

这里x是某种状态，y 是某种预观测。下面例子中 x 代表门开关，y 代表距离z我们称P(y|x)为causal knowledge（因果知识），意即由x的已知情况，就可以推算y发生的概率，例如在图2的例子中，已知如果门开着，则z=0.5m的概率为0.6；如果门关着，则z=0.5m的的概率为0.3。P(x) 为prior knowledge，先验概率。可以设开关概率都是 0.5。P(x|y) 是基于观测对状态的诊断或推断。贝叶斯公式的本质就是利用causal knowledge和prior knowledge来进行状态推断或推理

2.2 贝叶斯公式的计算

可以把分母项看成归一化系数 $\eta$ $$\begin{array}{l}P(x|y)=\frac{P(y|x)P(x)}{P(y)}=\eta P(y|x)P(x)\\ \eta =P(y{)}^{-1}=\frac{1}{\sum _{x}P(y|x)P(x)}\end{array}$$ Algorithm: $$\begin{array}{l}\forall x:\mathrm{a}\mathrm{u}{\mathrm{x}}_{x|y}=P(y|x)P(x)\\ \eta =\frac{1}{\sum _x\mathrm{a}\mathrm{u}{\mathrm{x}}_{x|y}}\\ \forall x:P(x|y)=\eta \mathrm{a}\mathrm{u}{\mathrm{x}}_{x|y}\end{array}$$

2.3 贝叶斯公式融合多种观测

$$\begin{array}{l}P(x|y,z)=\frac{P(x,y,z)}{P(y,z)}\\ =\frac{P(y|x,z)p(x,z)}{P(y,z)}\\ =\frac{P(y|x,z)p(x|z)p(z)}{P(y|z)p(z)}\\ =\frac{P(y|x,z)p(x|z)}{P(y|z)}\end{array}$$

所以，在预观测 y, z 条件下 x 发生的概率： $$P(x|y,z)=\frac{P(y|x,z)P(x|z)}{P(y|z)}$$

2.4 贝叶斯递推公式

由此，我们来推导贝叶斯滤波的递推公式： $$P(x|z_1,\dots ,z_n)=?$$ 再由Markov属性，在x已知的情况下，$z_n$ 同 $\{z_{n-1},\dots ,z_1\}$ 无关 $$\begin{array}{c}P(x|z_1,\dots ,z_n)=\frac{P(z_n|x,z_1,\dots ,z_{n–1})P(x|z1,\dots ,z_{n–1})}{P(z_n|z_1,\dots ,z_{n–1})}\\ =\frac{P(z_n|x)P(x|z1,\dots ,z_{n–1})}{P(z_n|z_1,\dots ,z_{n–1})}\end{array}$$ 所以： $$ \begin{array}{*{20}{l}}

{P(x|{z_1}, \ldots ,{z_n})}&{ = \frac{{P({z_n}|x);P(x|{z_1}, \ldots ,{z_{n{\rm{ - }}1}})}}{{P({z_n}|{z_1}, \ldots ,{z_{n - 1}})}}}\ {}&{ = {\eta n};P({z_n}|x);P(x|{z_1}, \ldots ,{z{n - 1}})}\ {}&\begin{array}{l} = {\eta n};P({z_n}|x);{\eta {n - 1}}P({z{n - 1}}|x)P(x|{z_1}, \ldots ,{z{n - 2}})\ = {\eta _1} \cdots {\eta n};\prod\limits{i = 1…n} {P({z_i}|x)} ;P(x) \end{array}

\end{array} $$

3. 如何融入动作

在实际问题中，对象总是处在一个动态变化的环境中，例如：

机器人自身的动作影响了环境状态其它对象，比如人的动作影响了环境状态或者就是简单的环境状态随着时间发生了变化。

如何在Bayes模型中来描述动作的影响呢?

首先，动作所带来的影响也总是具有不确定性的其次，相比于观测，动作一般会使得对象的状态更为模糊（或更不确定）。

我们用u来描述动作，在 x’ 状态下，执行动作 u 后，对象状态变成 x 的概率为： $$P(x|u,x’)$$ 动作对状态的影响一般由状态转移模型来描述。如图3所示，表示了“关门”这个动作对状态影响的转移模型。这个状态转移模型表示：关门这个动作有0.1的失败概率，所以当门是open状态时，执行“关门”动作，门有0.9的概率转为closed状态，有0.1的概率保持在open状态。门是closed的状态下，执行“关门”动作，门仍然是关着的。

执行某一动作后，计算动作后的状态概率，需要考虑动作之前的各种状态情况，把所有情况用全概率公式计算：

连续情况下 $$P(x|u)=\int P(x|u,x^{\prime })P(x^{\prime })d{x}^{\prime }$$

离散情况下 $$P(x|u)=\sum P(x|u,x^{\prime })P(x^{\prime })$$

例3：在例2的基础上，如果按照图3所示的状态转移关系，机器人执行了一次关门动作， 计算动作后门开着的概率？ $$\begin{array}{ll}P(open|u)& =\sum P(open|u,x^{\prime })P(x^{\prime })\\ & =P(open|u,open)P(open)\\ & +P(open|u,closed)P(closed)\\ & =\frac{1}{10}\ast 0.8+\frac{0}{1}\ast 0.2=0.08\end{array}$$

$$\begin{array}{ll}P(closed|u)& =\sum P(closed|u,x^{\prime })P(x^{\prime })\\ & =P(closed|u,open)P(open)\\ & +P(closed|u,closed)P(closed)\\ & =\frac{9}{10}\ast 0.8+\frac{1}{1}\ast 0.2=0.92\end{array}$$

所以门还开着的概率为 0.08 。

贝叶斯滤波算法

贝叶斯滤波推导：

Markov性假设: t 时刻状态仅由 t-1 时刻状态 $x_{t-1}$ 以及t时刻动作 $u_t$ 决定，t时刻观测仅由 t 时刻状态决定。

静态环境，即对象周边的环境假设是不变的

观测噪声、模型噪声等是相互独立的

推导过程：

$\eta$ 是求和的倒数

3.2 贝叶斯滤波算法

由推导可以看出贝叶斯算法两个步骤，第一是基于 $x_{t-1},u_t$ 预测 $x_t$ ，第二是基于观测 $z_t$ 更新状态 $x_t$ 。

Bayes algorithm

3.3 多个观测 $z_1,z_2,\dots$

可以把每一个观测看作相互独立，对这些观测依次更新状态。

贝叶斯滤波matlab程序实现

问题：一个一维空间里，有一个小机器人，可以测量距离前后墙壁的距离(测不准，高斯误差)，可以前后移动(移动成功和不成功)，用贝叶斯滤波对机器人进行定位。

下面是关键函数贝叶斯滤波，主要是实现下面的公式：

对于给定观测： $$bel\left(x_t\right)=\eta p\left(z_t\mid x_t\right)\overline{bel}\left(x_t\right)$$

对于给定动作： $$P(x|u)=\sum P(x|u,x^{\prime })P(x^{\prime })$$

function Belief = Bayesian_Filter(Belief,flag,SenseProb,TModel,X) n = size(Belief,1); if(flag==0) % observation %根据S的感知结果，计算感知概率 disp('receive observation'); Belief = Belief .* SenseProb(1, :).'; Belief = Belief ./ sum(Belief); Belief = Belief .* SenseProb(2, :).'; Belief = Belief ./ sum(Belief); elseif(flag==1) % action disp('receive action 1'); u=1; Belief_update = zeros(n, 1); for i1 = X tProb = TransitionModel(TModel, i1, u).'; Belief_temp = Belief .* tProb; Belief_update(i1) = sum(Belief_temp); end Belief = Belief_update; elseif(flag==2) % action disp('receive action 2'); u=2; Belief_update = zeros(n, 1); for i1 = X tProb = TransitionModel(TModel, i1, u).'; Belief_temp = Belief .* tProb; Belief_update(i1) = sum(Belief_temp); end Belief = Belief_update; else disp('t is invalid'); end disp({'置信度之和应为1：',sum(Belief), '\n真实位置:', groundTruth}); end

下面是全部代码

function BayesianFilterRobot() %% Setting up the environment % Yongcai Wang, ycw@ruc.edu.cn % 2019-9-20 % A robot is moving along a corridor of 20 meters. X=1:20; % Two sensors are deployed at the two ends of the corridor. Pos(S)=[0,21]; % The robot moves along the corridor from 1 to 20. % The motions include moving right, moving left and stand still. figure; set(figure(1) ,'KeyPressFcn',@keyhandler,'Name','Bayesian Update'); nGrids = 20; X=1:nGrids; Belief=1/nGrids*ones(nGrids,1); %初始化的机器人位置置信度 groundTruth = 1; %机器人的真实位置； %%两个传感器，分别检测到两端墙的距离，传感器的是0均值，方差为2 Sensor(1)=struct('toWall',0,'Sigma',2) Sensor(2)=struct('toWall',21,'Sigma',2) %% 获得传感器的读数 SensorData = TakeSensorReadings(groundTruth); %% 绘制机器人位置置信度分布 updateWorld(Belief); %% 定义两种动作的概率转移矩阵 nAction = 2; TModel = zeros(nAction,nGrids, nGrids); for i=1:1:nGrids-2 TModel(1,i,i)=0.1; TModel(1,i,i+1)=0.7; TModel(1,i,i+2)=0.2; end TModel(1,nGrids,nGrids)=1; TModel(1,nGrids-1,nGrids)=0.9; TModel(1,nGrids-1,nGrids-1)=0.1; for i=3:1:nGrids TModel(2,i,i)=0.1; TModel(2,i,i-1)=0.7; TModel(2,i,i-2)=0.2; end TModel(2,1,1)=1; TModel(2,2,1)=0.9; TModel(2,2,2)=0.1; disp('start'); %% 响应键盘动作的主函数 function keyhandler(src,evnt) %#ok<INUSL> if strcmp(evnt.Key,'rightarrow') SensorData = TakeSensorReadings(groundTruth); SenseProb = SensingModel(SensorData, X, Sensor); Belief=Bayesian_Filter(Belief,0,SenseProb,TModel,X); groundTruth=MoveRight(groundTruth,nGrids); Belief=Bayesian_Filter(Belief,1,SenseProb,TModel,X); updateWorld(Belief); elseif strcmp(evnt.Key,'leftarrow') SensorData = TakeSensorReadings(groundTruth); SenseProb = SensingModel(SensorData, X, Sensor); Belief=Bayesian_Filter(Belief,0,SenseProb,TModel,X); groundTruth=MoveLeft(groundTruth,nGrids); Belief=Bayesian_Filter(Belief,2,SenseProb,TModel,X); updateWorld(Belief) elseif strcmp(evnt.Key,'uparrow') || strcmp(evnt.Key,'downarrow') SensorData = TakeSensorReadings(groundTruth); SenseProb = SensingModel(SensorData, X, Sensor); Belief=Bayesian_Filter(Belief,0,SenseProb,TModel,X); updateWorld(Belief); end end %% 贝叶斯滤波 %@para: flag=0, 根据传感器的观测，进行置信度更新； % flag=1, 响应move right,进行置信度预测; % flag=2, 响应move left，进行置信度预测; function Belief = Bayesian_Filter(Belief,flag,SenseProb,TModel,X) n = size(Belief,1); if(flag==0) % observation %根据S的感知结果，计算感知概率 disp('receive observation'); % 计算左边感知 Belief = Belief .* SenseProb(1, :).'; Belief = Belief ./ sum(Belief); % 计算右边感知 Belief = Belief .* SenseProb(2, :).'; Belief = Belief ./ sum(Belief); elseif(flag==1) % action disp('receive action 1'); u=1; %TODO: 因为转移矩阵为上三角，可以对计算简化 Belief_update = zeros(n, 1); for i1 = X tProb = TransitionModel(TModel, i1, u).'; Belief_temp = Belief .* tProb; Belief_update(i1) = sum(Belief_temp); end Belief = Belief_update; elseif(flag==2) % action disp('receive action 2'); u=2; %TODO: 因为转移矩阵为下三角，可以对计算简化 Belief_update = zeros(n, 1); for i1 = X tProb = TransitionModel(TModel, i1, u).'; Belief_temp = Belief .* tProb; Belief_update(i1) = sum(Belief_temp); end Belief = Belief_update; else disp('t is invalid'); end disp({'置信度之和应为1：',sum(Belief), '\n真实位置:', groundTruth}); end %% 获得传感器读数 function St = TakeSensorReadings(x) s1=abs(Sensor(1).toWall-x)+(rand-0.5)*5; s2=abs(Sensor(2).toWall-x)+(rand-0.5)*5; St=[s1;s2]; end %% 更新置信度绘制 function updateWorld(Belief) imagesc(Belief'); axis([0,21,-5,5]); end %% 响应右移操作 function y=MoveRight(x, nGrids) disp('moving right'); if (x==nGrids) y=x; elseif(x==nGrids-1) if(rand<=0.1) y=x; else y=x+1; end else if(rand<=0.1) y=x; elseif(rand<0.1) y=x+2; else y=x+1; end end end %% 响应左移操作 function y=MoveLeft(x, nGrids) disp('moving left'); if (x==1) y=x; elseif(x==2) if(rand<=0.1) y=x; else y=x-1; end else if(rand<=0.1) y=x; elseif(rand<0.1) y=x-2; else y=x-1; end end end %% 每一种传感器对应一个感知模型，输出某个SensorData情况下， function p_z_x= SensingModel(SensorData, X, Sensor) %如果对应传感器没有感知结果，返回nan，否则返回一个测量值 for j=1:1:20 p_z_x(1,j)=normpdf(SensorData(1), abs(Sensor(1).toWall-X(j)), Sensor(1).Sigma); p_z_x(2,j)=normpdf(SensorData(2), abs(Sensor(2).toWall-X(j)), Sensor(2).Sigma); end end %% 每一种Action对应一个状态转移图，状态转移图存储在GT中 function tProb =TransitionModel(GT, i, u) %返回动作u对应的状态转移图中，由x_old状态到x_new状态的转移概率； tProb=GT(u,:,i); end end

参考

细说贝叶斯滤波

离散型贝叶斯滤波python编程代码实践

【易懂教程】我是如何十分钟理解与推导贝叶斯滤波(Bayes Filter)算法？