Particle Filter 粒子滤波 原理以及python实践
来源:互联网 发布:rhino软件适合做游戏 编辑:程序博客网 时间:2024/05/17 21:14
本笔记学习于优达学城 Artificial Intelligence for Robotics课程
什么是粒子滤波器,请看下面几幅图片
在整个空间内有无数关于机器人的位置坐标以及方向的粒子 [x,y,orientation],随着机器人的移动感知周围空间,这些粒子将获得自己的权重,然后进行筛选,最后粒子逐渐收敛到一起。这个就是粒子滤波器的基本原理。
首先创建好一个机器人的类,机器人可以移动,设定方向,设定误差
# -*- coding: utf-8 -*- from math import *import random#机器人四个参照物landmarks = [[20.0, 20.0], [80.0, 80.0], [20.0, 80.0], [80.0, 20.0]]#地图大小world_size = 100.0class robot: def __init__(self): self.x = random.random() * world_size self.y = random.random() * world_size self.orientation = random.random() * 2.0 * pi #给机器人初试化一个坐标和方向 self.forward_noise = 0.0; self.turn_noise = 0.0; self.sense_noise = 0.0; def set(self, new_x, new_y, new_orientation):#设定机器人的坐标 方向 if new_x < 0 or new_x >= world_size: raise ValueError, 'X coordinate out of bound' if new_y < 0 or new_y >= world_size: raise ValueError, 'Y coordinate out of bound' if new_orientation < 0 or new_orientation >= 2 * pi: raise ValueError, 'Orientation must be in [0..2pi]' self.x = float(new_x) self.y = float(new_y) self.orientation = float(new_orientation) def set_noise(self, new_f_noise, new_t_noise, new_s_noise): # makes it possible to change the noise parameters # this is often useful in particle filters #设定一下机器人的噪声 self.forward_noise = float(new_f_noise); self.turn_noise = float(new_t_noise); self.sense_noise = float(new_s_noise); def sense(self):#测量机器人到四个参照物的距离 可以添加一些高斯噪声 Z = [] for i in range(len(landmarks)): dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2) dist += random.gauss(0.0, self.sense_noise) Z.append(dist) return Z def move(self, turn, forward): #机器人转向 前进 并返回更新后的机器人新的坐标和噪声大小 if forward < 0: raise ValueError, 'Robot cant move backwards' # turn, and add randomness to the turning command orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise) orientation %= 2 * pi # move, and add randomness to the motion command dist = float(forward) + random.gauss(0.0, self.forward_noise) x = self.x + (cos(orientation) * dist) y = self.y + (sin(orientation) * dist) x %= world_size # cyclic truncate y %= world_size # set particle res = robot() res.set(x, y, orientation) res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise) return res def Gaussian(self, mu, sigma, x): # calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2)) def measurement_prob(self, measurement): # calculates how likely a measurement should be #计算出的距离相对于正确正确的概率 离得近肯定大 离得远就小 prob = 1.0; for i in range(len(landmarks)): dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2) prob *= self.Gaussian(dist, self.sense_noise, measurement[i]) return prob让机器人移动一下吧
# 初始化一个机器人myrobot = robot()#设定噪声myrobot.set_noise(5.0,0.1,5.0)#设定初始位置myrobot.set(30,50,0.5)#打印位置方向print myrobot#打印与四个参照物的距离Z=myrobot.sense()print Z#机器人移动 myrobot=myrobot.move(pi/2,10.0)print myrobotZ=myrobot.sense()print Z
接下来就是粒子滤波部分,首先在运动初期给机器人初始化1000个位置粒子,这些粒子随机分布在整个地图,就上上面的第一张图片一样。然后机器人移动,粒子跟随移动,然后计算各个粒子的权重
myrobot=robot()myrobot.move(0.1,5.0)Z=myrobot.sense()N=1000#初始化一千个粒子p=[]for i in range( N):x=robot()x.set_noise(0.05,0.05,5.0)p.append(x)print len(p)p2=[]for i in range(N):p2.append(p[i].move(0.1,5.0))p=p2#计算各个粒子的权重w=[]for i in range(N):w.append(p[i].measurement_prob(Z))print w
下一篇 更新粒子滤波器 Resampling 降低粒子个数 挑选出重要的粒子 完成粒子滤波器的设计
阅读全文
0 0
- Particle Filter 粒子滤波 原理以及python实践
- Particle Filter 粒子滤波 原理以及python实践二
- 粒子滤波(Particle Filter)
- 粒子滤波(Particle Filter)
- 粒子滤波(Particle Filter)
- 粒子滤波(PF:Particle Filter)
- 粒子滤波(PF:Particle Filter)
- 粒子滤波(Particle Filter)概念浅谈
- 粒子滤波(PF:Particle Filter)
- 粒子滤波(Particle filter)matlab实现
- 粒子滤波(PF:Particle Filter)
- 粒子滤波(PF:Particle Filter)
- 基于粒子滤波的跟踪算法 Particle Filter Object Tracking
- 粒子滤波(PF:Particle Filter)目标跟踪
- 粒子滤波(Particle filter)算法简介及MATLAB实现
- 粒子滤波(Particle Filter)的通俗解释
- 粒子滤波(PF:Particle Filter与卡尔曼滤波(Kalman Filter)相比较 (转载)
- 粒子滤波(Particle Filter)在目标跟踪(Object Tracking)中的应用
- spring 父子容器
- 关于Eclipse链接SQLServer2008时运行java出现的用户 ‘sa’ 登录失败问题!
- requests.exceptions.SSLError: [SSL: CERTIFICATE_VERIFY_FAILED] certificate verify failed (_ssl.c:581
- 关键字定义
- Python随机读取文件
- Particle Filter 粒子滤波 原理以及python实践
- DOS命令大全
- Javascript深浅拷贝的原理
- WebStorm下载,注册码,配色,主题,全解。
- nginx_配置范例_包括django,tornado,tornado_websock
- strcat源代码汇总
- jfinal数据库连接txt文档
- dfs序详解
- c++实验6-矩阵求和