# Set up environment #args
beacons = [
Beacon(np.array([0.2, 0.2])),
Beacon(np.array([1.2, 0.5])),
Beacon(np.array([0.2, 0.8]))
]
#obstacles = [RectangularObstacle(np.array([[0.75, 0.2], [0.75, 0.4], [0.85, 0.4], [0.85,0.2]], float).T),\
# RectangularObstacle(np.array([[0.5,0.85], [1.15,0.85], [1.15,0.6], [0.5,0.6]], float).T)]
obstacles = []
s = SimEnv2D(bounds=[-0.1, 1.5, -0.1, 1], beacons=beacons, obstacles=obstacles)
x0 = np.array([0, 0.5, 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([1e-5] * num_states)) #arg
Q[2, 2] = 1e-8 # Gets out of hand if noise in theta or phi
Q[3, 3] = 1e-8 # Can also add theta/phi to measurement like Sameep #TODO?
R = np.mat(np.diag([0.005] * num_measure)) #arg
R[3, 3] = 1e-9
from math import log from numpy.random import multivariate_normal as mvn from kalman_filter import ekf_update from covar import cov2vec, vec2cov from optimize import scp_solver_beliefs from scipy.io import loadmat import matplotlib.animation as animation # Set up environment #args beacons = [Beacon(np.array([0.3, 0.7])), Beacon(np.array([1, 0.7]))] s = SimEnv2D(bounds=[-0.1, 1.3, -0.1, 1], beacons=beacons) x0 = np.array([0, 0, np.pi / 4, 0]) car = SimpleCar(x0) car.attach_sensor(BeaconSensor(decay_coeff=100), 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([1e-3] * num_states)) # arg Q[2, 2] = 1e-8 # Gets out of hand if noise in theta or phi Q[3, 3] = 1e-8 # Can also add theta/phi to measurement like Sameep #TODO? R = np.mat(np.diag([0.0005] * num_measure)) # arg R[2, 2] = 1e-9
