def init():
init = Config['test']['initial_state']
x = np.array([[
init['x_0'], init['vx_0'], init['y_0'], init['vy_0'], init['theta_0']
]]).T
P = np.array([[0, 0, 0, 0, 0], [0, 0, 0, 0, 0], [0, 0, 0, 0, 0],
[0, 0, 0, 0, 0], [0, 0, 0, 0, 0]])
oss = models.OdometryStateSpace()
iss = models.IMUStateSpace()
rss = models.RobotStateSpace()
return x, P, oss, iss, rss
x = np.array([[
init_s['x_0'], init_s['vx_0'], init_s['y_0'], init_s['vy_0'],
init_s['theta_0']
]]).T
P = np.array([[init_p['p0_x'], 0, 0, 0, 0], [0, init_p['p0_vx'], 0, 0, 0],
[0, 0, init_p['p0_y'], 0, 0], [0, 0, 0, init_p['p0_vy'], 0],
[0, 0, 0, 0, init_p['p0_theta']]])
# Sensors
imu = sensors.IMU()
enc = sensors.Encoders()
# Models
rss = models.RobotStateSpace()
iss = models.IMUStateSpace()
oss = models.OdometryStateSpace()
N = 300
imu_a_x = 0
imu_a_y = 0
def log_data(file, data):
with open(file, "a") as f:
writer = csv.writer(f)
writer.writerow(data)
clock = Clock()
print("Starting...")