[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters
Field Trip
State Space & Belief Modality & Efficiency & Exact or Approximate
文章图片
Particle Filters
文章图片
Moving Robot
# Make a robot called myrobot that starts at
# coordinates 30, 50 heading north (pi/2).
# Have your robot turn clockwise by pi/2, move
# 15 m, and sense. Then have it turn clockwise
# by pi/2 again, move 10 m, and sense again.
#
# Your program should print out the result of
# your two sense measurements.
#
# Don't modify the code below. Please enter
# your code at the bottom.from math import *
import randomlandmarks= [[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 Zdef 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 resdef 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 beprob = 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 probdef __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))def eval(r, p):
sum = 0.0;
for i in range(len(p)): # calculate mean error
dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0)
dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0)
err = sqrt(dx * dx + dy * dy)
sum += err
return sum / float(len(p))####DON'T MODIFY ANYTHING ABOVE HERE! ENTER CODE BELOW ####myrobot = robot()
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/8e02ae8713d040bfa3dddc8f2b756981.jpg)
文章图片
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/ed6b4cc08c744bcf9dcd3d1067cce177.jpg)
文章图片
Add Noise
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/bc2ce82f1d304dda90d0489e43fe9f94.jpg)
文章图片
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/55907bd4246341b3af863a570019ea6f.jpg)
文章图片
Robot World robot that can sense the distances to different landmarks Creating Particles
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/d9d0517df4cb4f368007a166cc3233d1.jpg)
文章图片
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/3de0745bb13d46b7b1e7a5a973e201bb.jpg)
文章图片
A thousand virtual robots Robot Particles
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/c255b8548a9e423890141788c290ead9.jpg)
文章图片
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/2987641e71ce4c4eaa6a98bd054cd9a7.jpg)
文章图片
Importance Weight
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/374e967236cc43dda798f77becd6e2bf.jpg)
文章图片
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/c7b875945bb14409a058faf93fa5e382.jpg)
文章图片
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/7ee1654423a348009a53a2ec237337a6.jpg)
文章图片
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/e729841d85894e63bb86c962436ca0ca.jpg)
文章图片
Resampling
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/6c6025afb9f74dc4ad21765e54264381.jpg)
文章图片
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/044159437c244cf29cf9c39e16709726.jpg)
文章图片
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/f2e51ee4fd43444e89596800fe1a3ce9.jpg)
文章图片
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/f918c095c91744c88462c4c7fefe6e4e.jpg)
文章图片
Never Sampled
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/c95fba36ed2341579caec8ae289dbe8b.jpg)
文章图片
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/22c490ef1b05481281df70f3be551790.jpg)
文章图片
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/29a157a946f148a5b40e7227ce3e62b9.jpg)
文章图片
Filters
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/5c096aba17f540b3a1d37cba5cd8e0b8.jpg)
文章图片
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/63f5b2abe7fd43d993c672b244e8dd5b.jpg)
文章图片
http://robots.stanford.edu/papers/thrun.stanley05.html
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/6b283c8acc904bda951965d9f7c75ca6.jpg)
文章图片
【[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters】
2012
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/55929a580f4a4d4981cda73807293ed8.jpg)
文章图片
Circular Motion
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/a7f386b395d9404282b948b191336e6c.jpg)
文章图片
bicycle model:
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/df87a821e7a9418c8e463692c85d5730.jpg)
文章图片
if $\beta$ is small, we can approximate this as a straight motion
![[Artificial|[Artificial Intelligence for Robotics] {cs373} Lesson 8: Particle Filters](https://img.it610.com/image/info10/8ebdaeb3605c4965a70a5b40b7e41ae1.jpg)
文章图片
推荐阅读
- whlie循环和for循环的应用
- ffmpeg源码分析01(结构体)
- 【WORKFOR】最真的自己
- R|R for data Science(六)(readr 进行数据导入)
- performSelectorOnMainThread:withObject:waitUntilDone:参数设置为NO或YES的区别
- JavaScript|JavaScript — 初识数组、数组字面量和方法、forEach、数组的遍历
- Swift7|Swift7 - 循环、函数
- 65|65 - Tips for File Handling
- 40
- 360|360 将停止 StartCom 数字证书业务;微软的 Visual Studio Tools for AI 现已提供使用