from sensors import BeaconSensor from utils import mat2tuple import random from math import log from numpy.random import multivariate_normal as mvn from kalman_filter import ekf_update # Set up environment #args beacons = [Beacon(np.array([-1, -1])), Beacon(np.array([1, 1]))] beacons = beacons[1:2] s = SimEnv2D(bounds=[-2, 2, -2, 2], beacons=beacons) x0 = np.array([-1, 1, -np.pi / 4, 0]) car = SimpleCar(x0) car.attach_sensor(BeaconSensor(), lambda x: x[0:2]) s.add_robot(car) # Number of timesteps T = 20 #arg # Dynamics and measurement noise num_states = car.NX num_ctrls = car.NU num_measure = len(beacons) #arg/make part of robot observe Q = np.mat(np.diag([0.0005] * num_states)) #arg Q[2, 2] = 0 # Gets out of hand if noise in theta or phi Q[3, 3] = 0 R = np.mat(np.diag([0.00005] * num_measure)) * 0.001 #arg # Sample noise
beacons = beacons[1:]
obstacles = [
RectangularObstacle(
np.array([[-0.5, 0.3], [-0.5, 0.5], [-0.10, 0.5], [-0.10, 0.3]],
float).T),
RectangularObstacle(
np.array([[0.5, 0.5], [0.5, 0.7], [0.10, 0.7], [0.10, 0.5]], float).T)
]
obstacles = obstacles[0:1]
s = SimEnv2D(bounds=[-0.5, 0.5, -0.1, 1.2],
beacons=beacons,
obstacles=obstacles)
x0 = np.array([-0.4, 0.2, 0, 0])
car = SimpleCar(x0)
car.attach_sensor(BeaconSensor(decay_coeff=25), lambda x: x[0:2])
s.add_robot(car)
# Number of timesteps
T = 30 #arg
# Dynamics and measurement noise
num_states = car.NX
num_ctrls = car.NU
num_measure = len(beacons) + 1 #arg/make part of robot observe
Q = np.mat(np.diag([0.0000001] * num_states)) #arg
R = np.mat(np.diag([0.001] * num_measure)) #arg
R[2, 2] = 0.000005
# Sample noise
dynamics_noise = mvn([0] * num_states, Q, T - 1).T * 0 #FIXME
measurement_noise = mvn([0] * num_measure, R, T - 1).T * 0 #FIXME
print d
stop
print d['dat_states'].shape
print d['dat_ctrls'].shape
'''
# Set up environment #args
beacons=[Beacon(np.array([0.3,0.7])),
Beacon(np.array([0.8,-0.1]))]
obstacles = [CircularObstacle(0.5, 0.63, 0.08)]
s = SimEnv2D(bounds=[-0.1, 1.3, -0.1, 1], beacons=beacons, obstacles=obstacles)
x0 = d['dat_states'][:,0]
car = SimpleCar(x0)
car.attach_sensor(BeaconSensor(decay_coeff=25), lambda x: x[0:2])
s.add_robot(car)
# Number of timesteps
T = 30 #arg
# Dynamics and measurement noise
num_states = car.NX
num_ctrls = car.NU
num_measure = len(beacons)+2 #arg/make part of robot observe
Q = np.mat(np.diag([0.000001]*num_states)) #arg
Q[2,2] = 0 # Gets out of hand if noise in theta or phi
Q[3,3] = 0 # Can also add theta/phi to measurement like Sameep #TODO?
R = np.mat(np.diag([0.0005]*num_measure)) #arg
R[2,2] = 0.000005
from utils import mat2tuple
import random
from math import log
from numpy.random import multivariate_normal as mvn
from kalman_filter import ekf_update
# Set up environment #args
beacons=[Beacon(np.array([-1,-1])),
Beacon(np.array([1,1]))]
beacons = beacons[1:2]
s = SimEnv2D(bounds=[-2, 2, -2, 2], beacons=beacons)
x0 = np.array([-1, 1, -np.pi/4, 0])
car = SimpleCar(x0)
car.attach_sensor(BeaconSensor(), lambda x: x[0:2])
s.add_robot(car)
# Number of timesteps
T = 20 #arg
# Dynamics and measurement noise
num_states = car.NX
num_ctrls = car.NU
num_measure = len(beacons) #arg/make part of robot observe
Q = np.mat(np.diag([0.0005]*num_states)) #arg
Q[2,2] = 0 # Gets out of hand if noise in theta or phi
Q[3,3] = 0
R = np.mat(np.diag([0.00005]*num_measure))*0.001 #arg
# Sample noise
