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...")