%matplotlib inline
import numpy as np
from copy import copy
import math, random
import matplotlib.pyplot as plt # for plotting data
from matplotlib.patches import Ellipse # for drawing
### 変数 ###
# 実際の世界のシミュレーション
actual_x = np.array([0.0,0.0,0.0]) #ロボットの実際の姿勢
u = np.array([0.2,math.pi / 180.0 * 20]) #ロボットの移動
# モンテカルロ法のための変数
class Particle:
def __init__(self,w):
self.pose = np.array([0.0,0.0,0.0])
self.weight = w
def __repr__(self):
return "pose: " + str(self.pose) + " weight: " + str(self.weight)
particles = [Particle(1.0/100) for i in range(100)]
# 一個プリントしてみましょう
print(particles[0])
pose: [ 0. 0. 0.] weight: 0.01
前回のをコピペ
def f(x_old,u):
pos_x, pos_y, pos_theta = x_old
act_fw, act_rot = u
act_fw = random.gauss(act_fw,act_fw/10)
dir_error = random.gauss(0.0, math.pi / 180.0 * 3.0)
act_rot = random.gauss(act_rot,act_rot/10)
pos_x += act_fw * math.cos(pos_theta + dir_error)
pos_y += act_fw * math.sin(pos_theta + dir_error)
pos_theta += act_rot
return np.array([pos_x,pos_y,pos_theta])
これで準備OK
import copy
path = [actual_x]
particle_path = [copy.deepcopy(particles)]
for i in range(10):
actual_x = f(actual_x,u)
path.append(actual_x)
for p in particles:
p.pose = f(p.pose,u)
particle_path.append(copy.deepcopy(particles))
print(path[0])
print(particle_path[0])
print(path[10])
print(particle_path[10])
[ 0. 0. 0.] [pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01, pose: [ 0. 0. 0.] weight: 0.01] [ 0.01614545 1.11655664 3.58506062] [pose: [ 0.01734599 1.20518118 3.38961758] weight: 0.01, pose: [ 0.14067426 1.09540245 3.48434512] weight: 0.01, pose: [-0.03675291 1.01050254 3.63560435] weight: 0.01, pose: [-0.11952947 1.104785 3.56950413] weight: 0.01, pose: [ 0.03209341 1.10986851 3.45209625] weight: 0.01, pose: [ 0.09789036 1.20811542 3.41238955] weight: 0.01, pose: [-0.04804419 1.0742375 3.55197697] weight: 0.01, pose: [-0.13056078 1.12235485 3.51665937] weight: 0.01, pose: [-0.08359872 1.06479677 3.59629178] weight: 0.01, pose: [-0.0143955 1.14247446 3.40402246] weight: 0.01, pose: [ 0.15079481 1.17775277 3.2638102 ] weight: 0.01, pose: [ 0.04332535 1.18890611 3.47796401] weight: 0.01, pose: [ 0.0070267 1.00853635 3.51737211] weight: 0.01, pose: [ 0.00710169 1.12465871 3.48126149] weight: 0.01, pose: [ 0.12671985 1.26209876 3.4756379 ] weight: 0.01, pose: [-0.01486352 1.03847194 3.56298045] weight: 0.01, pose: [-0.0282506 0.99847902 3.64203359] weight: 0.01, pose: [-0.077858 1.14127602 3.54841263] weight: 0.01, pose: [-0.0664697 1.06792518 3.61256779] weight: 0.01, pose: [ 0.00922352 1.11377798 3.49758701] weight: 0.01, pose: [-0.09459638 1.13872486 3.48016417] weight: 0.01, pose: [-0.05171431 0.9413085 3.55444682] weight: 0.01, pose: [ 0.1484142 1.26004352 3.27679085] weight: 0.01, pose: [-0.04656941 1.13615364 3.40581222] weight: 0.01, pose: [-0.02464148 1.18969382 3.41803873] weight: 0.01, pose: [ 0.05720068 1.16731752 3.4368182 ] weight: 0.01, pose: [ 0.23441378 1.16301533 3.34262301] weight: 0.01, pose: [ 0.05725734 1.14356817 3.28522726] weight: 0.01, pose: [ 0.12428795 1.14703828 3.45743417] weight: 0.01, pose: [-0.01254756 1.08998504 3.6505922 ] weight: 0.01, pose: [ 0.04020105 1.17224547 3.33504312] weight: 0.01, pose: [-0.2604777 1.05467473 3.64033609] weight: 0.01, pose: [ 0.00572072 1.10535325 3.47412964] weight: 0.01, pose: [-0.00864797 1.16449334 3.46556097] weight: 0.01, pose: [ -9.07603286e-04 1.07481671e+00 3.70750228e+00] weight: 0.01, pose: [ 0.07480832 1.05916152 3.52981378] weight: 0.01, pose: [ 0.06722957 1.2218492 3.39132964] weight: 0.01, pose: [-0.03387826 1.12393226 3.55223523] weight: 0.01, pose: [-0.0174824 1.12660374 3.52692288] weight: 0.01, pose: [-0.05870051 1.0408486 3.6324189 ] weight: 0.01, pose: [ 0.08797761 1.16636222 3.40132655] weight: 0.01, pose: [ 0.0825788 1.12815353 3.43219542] weight: 0.01, pose: [ 0.06286047 1.19418831 3.38621449] weight: 0.01, pose: [-0.12570807 1.00598377 3.64162192] weight: 0.01, pose: [ 0.02436705 1.01954888 3.64516018] weight: 0.01, pose: [-0.16816711 1.14611102 3.54321306] weight: 0.01, pose: [-0.07882103 1.06863102 3.55946962] weight: 0.01, pose: [ 0.08570043 1.09476847 3.36808281] weight: 0.01, pose: [ 0.03080596 1.17446359 3.2874088 ] weight: 0.01, pose: [ 0.01906103 1.17370571 3.53835583] weight: 0.01, pose: [-0.04039951 1.14913523 3.5337639 ] weight: 0.01, pose: [-0.05202592 1.15015385 3.54717778] weight: 0.01, pose: [ 0.12142147 1.16730544 3.46323377] weight: 0.01, pose: [-0.03764508 1.13306468 3.52495051] weight: 0.01, pose: [ 0.14570327 1.20237252 3.45213784] weight: 0.01, pose: [ 0.04602506 1.06368099 3.51479391] weight: 0.01, pose: [-0.16225531 1.04383968 3.57096711] weight: 0.01, pose: [-0.03108543 1.16616758 3.47207338] weight: 0.01, pose: [-0.04768975 1.19465629 3.50537609] weight: 0.01, pose: [-0.0489686 1.09667159 3.4859566 ] weight: 0.01, pose: [-0.03564458 1.11614241 3.32290167] weight: 0.01, pose: [ 0.02173056 1.06339176 3.67430178] weight: 0.01, pose: [-0.13355162 1.00669949 3.7288648 ] weight: 0.01, pose: [ 0.05846703 1.07904908 3.65362641] weight: 0.01, pose: [ 0.02857776 1.205207 3.41763767] weight: 0.01, pose: [ 0.21340256 1.2461012 3.37405234] weight: 0.01, pose: [-0.09814257 1.12557724 3.48025132] weight: 0.01, pose: [-0.00594669 1.20940314 3.51234865] weight: 0.01, pose: [-0.0946811 1.07292629 3.63943214] weight: 0.01, pose: [-0.13120766 1.11856025 3.58455391] weight: 0.01, pose: [ 0.04556234 1.11747266 3.48304896] weight: 0.01, pose: [-0.01078967 1.13391111 3.44343668] weight: 0.01, pose: [ 0.02748639 1.28010546 3.38682889] weight: 0.01, pose: [ 0.08043834 1.15860471 3.38329211] weight: 0.01, pose: [ 0.07526524 1.10465433 3.4765806 ] weight: 0.01, pose: [-0.00736167 1.10547357 3.49457588] weight: 0.01, pose: [-0.07061461 1.12996688 3.58193182] weight: 0.01, pose: [-0.11947022 1.09319107 3.53328906] weight: 0.01, pose: [-0.04813927 1.12332923 3.54283598] weight: 0.01, pose: [ 0.14050502 1.23118722 3.39338234] weight: 0.01, pose: [-0.0256233 1.16561213 3.57678437] weight: 0.01, pose: [-0.07127631 0.94807556 3.70327368] weight: 0.01, pose: [ 0.0976791 1.18405202 3.37984228] weight: 0.01, pose: [ 0.03978661 1.2653564 3.44929203] weight: 0.01, pose: [ 0.09686846 1.19513706 3.35230642] weight: 0.01, pose: [-0.07058766 1.11026733 3.52443772] weight: 0.01, pose: [ 0.08259952 1.11230701 3.36883253] weight: 0.01, pose: [-0.01364817 1.03883002 3.55118194] weight: 0.01, pose: [ 0.00362118 1.11112611 3.52194495] weight: 0.01, pose: [ 0.07198805 1.17571155 3.42233676] weight: 0.01, pose: [-0.07167549 1.09892701 3.52359114] weight: 0.01, pose: [ 0.06740852 1.15074433 3.49684461] weight: 0.01, pose: [ 0.07599775 1.15525312 3.44088293] weight: 0.01, pose: [ 0.13875945 1.2644619 3.36819546] weight: 0.01, pose: [ 0.08270819 1.1609582 3.43980574] weight: 0.01, pose: [ 0.04604382 1.18831354 3.41534129] weight: 0.01, pose: [-0.11456185 1.00344149 3.56744868] weight: 0.01, pose: [ 0.00890828 1.09800655 3.38303898] weight: 0.01, pose: [-0.04861796 1.07882509 3.63858782] weight: 0.01, pose: [ 0.00546514 1.06600912 3.55540792] weight: 0.01]
def draw(pose,particles):
fig = plt.figure(i,figsize=(8, 8))
sp = fig.add_subplot(111, aspect='equal')
sp.set_xlim(-1.0,1.0)
sp.set_ylim(-0.5,1.5)
xs = [e.pose[0] for e in particles]
ys = [e.pose[1] for e in particles]
vxs = [math.cos(e.pose[2]) for e in particles]
vys = [math.sin(e.pose[2]) for e in particles]
plt.quiver(xs,ys,vxs,vys,color="blue",label="particles")
plt.quiver([pose[0]],[pose[1]],[math.cos(pose[2])],[math.sin(pose[2])],color="red",label="actual robot motion")
for i,p in enumerate(path):
draw(path[i],particle_path[i])