python中值滤波算法_Python实现卡尔曼滤波算法之贝叶斯滤波

Python实现卡尔曼滤波算法之贝叶斯滤波

作者：yangjian

卡尔曼滤波器属于贝叶斯滤波器的一种特例，本文主要讲解贝叶斯滤波原理及其算法的python实现。

先来看下贝叶斯公式

贝叶斯公式

$p(x∣y) = \frac{p(y | x)p(x)}{p(y)}$

$\begin{array}{l} P(x\,\left| {\,y} \right.) = \frac{{P(y|x)\,\,P(x)}}{{P(y)}} = \eta \;P(y|x)\,P(x)\\ \eta = P{(y)^{ - 1}} = \frac{1}{{\sum\limits_x {P(y|x)} P(x)}} \end{array}$

$p(x∣y)$ ：后验概率（posterior）

$p(y∣x)$ ：似然（likelyhood）

$p(x)$ ：先验概率（prior）

$p(y)$ ：置信（evidence）

Bayes滤波算法

贝叶斯滤波算法的推导过程：

$Bel({x_t}) = P({x_t}|{u_1},{z_1}\; \ldots ,{u_t},{z_t})$

$= \eta {\kern 1pt} P({z_t}|{x_t},{u_1},{z_1}, \ldots ,{u_t})\;P({x_t}|{u_1},{z_1},\; \ldots ,{u_t})$

$= \eta \;{\kern 1pt} P({z_t}|{x_t})\;P({x_t}|{u_1},{z_1},\; \ldots ,{u_t})$

$\!=\! \eta P({z_t}|{x_t})\int {P({x_t}|{u_1},{z_1},\! \ldots ,{u_t},{x_{t - 1}})} P({x_{t - 1}}|{u_1},{z_1}, \ldots ,{u_t})d{x_{t - 1}})$

$= \eta P({z_t}|{x_t})\int {P({x_t}|{u_t},{x_{t - 1}})} P({x_{t - 1}}|{u_1},{z_1}, \ldots ,{u_t})d{x_{t - 1}})\;$

$= \eta P({z_t}|{x_t})\int {P({x_t}|{u_t},{x_{t - 1}}){\mkern 1mu} } P({x_{t - 1}}|{u_1},{z_1}, \ldots ,{z_{t - 1}})d{x_{t - 1}})\;$

其中第一步采用贝叶斯公式展开，第二步使用Markov性质(

$z_t$ 仅由

$x_t$ 决定)；第三步使用全概率公式对

$x_{t−1}$ 进行展开；第四步继续使用Markov性质(

$x_t$ 仅由

$x_{t−1}$ 和

$u_t$ 决定)；第五步继续使用Markov性质，因为

$x_{t−1}$ 同

$u_t$ 无关，最终得到

$Bel(x_t)$ 的递推公式。

可见递推公式中分为两个步骤，

$\int {P({x_t}|{u_t},{x_{t - 1}})} Bel({x_{t - 1}})\;d{x_{t - 1}}$ 部分是基于

$x_{t−1}$ ,

$u_t$ 预测

$x_t$ 的状态

$\eta P({z_t}|{x_t})$ 部分是基于观测

$z_t$ 更新状态

$x_t$

算法设定

算法设定由上述推导和示例，我们可以给出贝叶斯滤波的算法，算法的输入输出设定如下。

系统输入 1到t时刻的状态观测和动作：

$dt={u_1,z_1…,u_t,z_t}$

观测模型：

$P(z|x)$

动作的状态转移模型：

$P(x|u,x^′)$

系统状态的先验概率分布

$P(x)$

期望输出

计算状态的后验概率，称为状态的置信概率：

$Bel(x_t)=P(x_t|u_1,z_1…,u_t,z_t)$

算法实现

以跟踪小车位置为例，小车在封闭的环形跑道内运动，假设环形跑道只有10个位置，将这些位置从0至9编号，其中的一些位置旁边有打开的门，一些位置没有，小车带有超声波传感器，可以测量小车两侧的距离，以判断所处位置是否有门。

$x_t$ 是1到t时刻小车的位置状态，

$z_t$ 是1到t时刻的超声波传感器测量到当前位置是否有门。

$P(x|z)$ 表示超声波传感器测量到某结果时（有门或无门），处于某位置的概率，即后验概率（posterior）

$P(z|x)$ 表示小车处于某一位置时，超声波传感器测量到是有门还是无门的概率，即似然（likelyhood）

滤波算法的第一部分：

$\int {P({x_t}|{u_t},{x_{t - 1}})} Bel({x_{t - 1}})\;d{x_{t - 1}}$ 部分是状态预测，基于

$x_{t-1},u_t$ 预测

$x_t$ 的状态，这个步骤用卷积计算。

概率论中,两个统计独立变量X与Y的和的概率密度函数是X与Y的概率密度函数的卷积。在这个例子中，我们正在使用传感器的误差函数修改概率分布。

卷积定义为

$(f \ast g) (t) = \int_0^t \!f(\tau) \, g(t-\tau) \, \mathrm{d}\tau$

积分用于连续函数，但是我们的例子使用的是离散函数。用求和替换积分，离散卷积形式：

$(f \ast g) [t] = \sum\limits_{\tau=0}^t \!f[\tau] \, g[t-\tau]$

假设小车也有速度传感器，速度传感器指示小车是在匀速运动，小车的运动模型即是动作的状态转移模型。假设速度传感器有一定的误差，得出的的运动测量值只有80％正确的，有10％可能超出实际一个位置，有10％可能在实际位置的前一位置。使用0.8表示移动到正确位置的概率，使用0.1表示欠调，使用0.1表示过冲。定义一个数组kernel=[0.1, 0.8, 0.1]。预测这一步需要做的就是编写一个循环，遍历概率分布数组中的每个元素，乘以kernel，然后对结果求和。

卷积实现的代码：

def predict_convolution(pdf, kernel):

N = len(pdf)

kN = len(kernel)

width = int((kN - 1) / 2)

prior = np.zeros(N)

for i in range(N):

for k in range (kN):

index = (i + width-k) % N

prior[i] += pdf[index] * kernel[k]

return prior

滤波算法的第二部分：

$\eta P({z_t}|{x_t})$ 是基于观测

$z_t$ 更新状态

$x_t$ ，先计算似然，然后与预测相乘，最后归一化，执行状态更新，

这里的

$z_t$ 是观测，是超声波传感器测量到当前位置是否有门。假设我们获得了超声波传感器的读数，并以知该传感器正确比错误的可能性高9倍。则应该在有门的地方按9倍的比例缩放概率分布。这样做之后结果将不再是概率分布，因为概率之和不等于1.0，需要通过将每个元素除以列表中所有元素的总和来完成归一化，乘以

$\eta$ 就是归一化的过程。

# 归一化分布“ pdf”，使其总计为1.0。

def normalize(pdf):

pdf /= sum(np.asarray(pdf, dtype=float))

return pdf

# 更新状态

def update(likelihood, prior):

posterior = prior * likelihood

return normalize(posterior)

def lh_hallway(hall, z, z_prob):

try:

scale = z_prob / (1. - z_prob)

except ZeroDivisionError:

scale = 1e8

likelihood = np.ones(len(hall))

likelihood[hall == z] *= scale

return likelihood

def bar_plot(pos, x=None, ylim=(0,1), title=None, c='#30a2da',

**kwargs):

ax = plt.gca()

if x is None:

x = np.arange(len(pos))

ax.bar(x, pos, color=c, **kwargs)

if ylim:

plt.ylim(ylim)

plt.xticks(np.asarray(x), x)

if title is not None:

plt.title(title)

def discrete_bayes_sim(pos, kernel, zs, z_prob_correct):

N = len(hallway)

for i, z in enumerate(zs):

plt.subplot(steps, 2, i*2 + 1)

# 预测

prior = predict_convolution(shift(pos, 1, cval=0.), kernel)

bar_plot(hallway, c='k')

bar_plot(prior, ylim=(0, 1.0), c='#ff8015', title='step {} 预测'.format(i + 1))

plt.axvline(i % N, lw=5)

plt.subplot(steps, 2, i*2 + 2)

likelihood = lh_hallway(hallway, z=z, z_prob=z_prob_correct)

pos = update(likelihood, prior)

bar_plot(hallway, c='k')

bar_plot(pos, ylim=(0, 1.0),title='step {} 更新'.format(i + 1))

plt.axvline(i % 10, lw=5)

plt.tight_layout()

plt.savefig('bys.png')

plt.close()

print('Final posterior:', pos)

steps = 7

kernel = (.1, .8, .1)

z_prob = 0.9

# 代表门实际位置的数组

hallway = np.array([1, 1, 0, 0, 1, 0, 0, 0, 1, 0])

# 观测数组z

measurements = [hallway[i % len(hallway)] for i in range(steps)]

# 初始化概率分布

pos = np.array([.1] * 10)

discrete_bayes_sim(pos, kernel, measurements, z_prob);

运行结果：

门口的位置用黑色绘制。粗的垂直线指示真实位置。先验用橙色绘制，后验用蓝色绘制。可以看到先验如何转移位置并降低确定性，而后验如何保持相同的位置并增加确定性，因为它合并了来自测量的信息。这里设定测量值的正确性z_prob = 0.9; 在预测过程中会降低对位置估计的确定性。但是每个预测之后都会将测量合并到估计中进行更新。此更新提高了确定性。更新步骤的输出被输入到下一个预测中，就这样一步步迭代实现了贝叶斯滤波的过程。

python中值滤波算法_Python实现卡尔曼滤波算法之贝叶斯滤波相关推荐

python实现排列组合公式算法_Python实现卡尔曼滤波算法之贝叶斯滤波
Python实现卡尔曼滤波算法之贝叶斯滤波作者:yangjian 卡尔曼滤波器属于贝叶斯滤波器的一种特例,本文主要讲解贝叶斯滤波原理及其算法的python实现. 先来看下贝叶斯公式贝叶斯公式 :后 ...
从贝叶斯滤波理论到容积卡尔曼滤波算法（CKF）详细推导及编程实现常转弯率模型估计。（matlab）
容积卡尔曼滤波(CKF)是由加拿大学者Arasaratnam和Haykin在2009年提出的.该算法的核心思想是针对非线性高斯系统,通过三阶球面径向容积准则来近似状态的后验均值和协方差,以保证在理论上 ...
滤波估计理论（一）——贝叶斯滤波
滤波估计理论(一)--贝叶斯滤波(Bayesian Filtering) 估计问题的建模状态空间模型概率模型贝叶斯估计方法批处理贝叶斯估计预测,滤波还是平滑? 贝叶斯滤波状态预测量测更新 ...
python中值滤波去除椒盐噪声_python 中值滤波,椒盐去噪,图片增强实例
受光照.气候.成像设备等因素的影响,灰度化后的图像存在噪声和模糊干扰,直接影响到下一步的文字识别,因此,需要对图像进行增强处理.图片预处理中重要一环就是椒盐去澡,通常用到中值滤波器进行处理,效果很好. ...
Python中值传递和引用传递区别
原文:http://blog.csdn.net/xuqiaobo/article/details/72236539 举例,函数参数如果是dic,都不需要返回值,原值就被改变了 def aa(dic): ...
9 概率机器人 Probabilistic Robotics 二值贝叶斯滤波占据栅格地图 occupancy grid mapping
文章目录 1 前言 2 二值贝叶斯滤波 2.1 理论基础 2.2 算法流程 2.3 重要公式推导 3 实例:占据栅格地图(occupancy grid mapping) 4 参考文献 1 前言如果通 ...
【算法系列】卡尔曼滤波算法
系列文章目录 ·[算法系列]卡尔曼滤波算法 ·[算法系列]非线性最小二乘求解-直接求解法 ·[算法系列]非线性最小二乘求解-梯度下降法 ·[算法系列]非线性最小二乘-高斯牛顿法 ·[算法系列]非线性最 ...
【SLAM基础入门】贝叶斯滤波、卡尔曼滤波、粒子滤波笔记（1）
贝叶斯滤波.卡尔曼滤波.粒子滤波 (https://www.bilibili.com/video/BV1HT4y1577g?spm_id_from=333.999.header_right.histo ...
【贝叶斯滤波与卡尔曼滤波】第四讲连续随机变量的贝叶斯公式
声明:[贝叶斯滤波与卡尔曼滤波]系列是博主对B站up主:忠厚老实的老王所分享教学内容的学习笔记,并且该系列每篇博客都会将博主听课后总结的纸质版笔记附于文末,供大家参考. B站up主:忠厚老实的老王是一 ...

python中值滤波算法_Python实现卡尔曼滤波算法之贝叶斯滤波

python中值滤波算法_Python实现卡尔曼滤波算法之贝叶斯滤波相关推荐

最新文章

热门文章