def find_kalman_controls(control,
heading,
omega=None,
winds=None,
plane_specs=None,
time_step=1):
control_x, control_z = 0, 0
if winds:
headings = []
plane_cs, plane_mass, plane_half_length = plane_specs
for wind in winds:
wind_speed, wind_heading = wind
wind_force = wind_speed * plane_cs
wind_torque = plane_half_length * wind_force * np.sin(
np.radians(wind_heading - heading))
wind_w = 0.1 * time_step * (wind_torque /
plane_mass) * plane_half_length
headings.append(heading + (omega + wind_w) * time_step)
wind_acc = wind_speed * plane_cs / plane_mass
control_x += wind_acc * np.cos(
np.radians(solver_heading(wind_heading)))
control_z += wind_acc * np.sin(
np.radians(solver_heading(wind_heading)))
control_x /= len(winds)
control_z /= len(winds)
heading = sum(headings) / len(headings)
control_x += control * np.cos(np.radians(solver_heading(heading)))
control_z += control * np.sin(np.radians(solver_heading(heading)))
return control_x, control_z
def solve_states2(initial_states,
desired_states,
center_line,
extern_conditions,
plane_specs,
constraints,
time_step=1,
sim_time=10):
acceleration_constraint, turning_constraint = constraints
rejection, init_velocity, init_heading = initial_states
desired_heading, desired_velocity = desired_states
init_heading = solver_heading(init_heading)
init_guess = formulate_guess(sim_time)
bounds = [(-acceleration_constraint, acceleration_constraint),
(-turning_constraint, turning_constraint)] * sim_time
rinit_x, rinit_y = rotate(0, rejection,
solver_heading(desired_heading) - 360)
state0 = [rinit_x, rinit_y, init_velocity, init_heading]
obj = formulate_objective(
state0,
center_line, [solver_heading(desired_heading), desired_velocity],
extern_conditions,
plane_specs,
time_step=time_step)
start_time = time.time()
result = opt.minimize(obj,
init_guess,
method='SLSQP',
bounds=bounds,
options={
'eps': 0.2,
'maxiter': 300
})
# print('-----------------------------------------')
# print('----', time.time() - start_time, 'seconds ----')
# print('-----------------------------------------\n')
states = compute_states(state0,
result.x,
extern_conditions[0],
plane_specs,
time_step=time_step)
states = get_states_controls(states)
return get_kalman_controls(states, time_step), result.fun
def compute_states(init_state,
controls,
wind_dynamics,
plane_specs,
time_step=1):
wind_speed, wind_heading = wind_dynamics
wind_heading = solver_heading(wind_heading)
plane_cs, plane_mass, plane_half_length = plane_specs
wind_force = wind_speed * plane_cs
wind_acc = wind_force / plane_mass
states = []
x, y, v, h = init_state
for i in range(0, len(controls), 2):
a, w = controls[i:i + 2]
states = np.concatenate((states, [x, y, v, h, a, w]))
vx = (v * np.cos(np.radians(h))) + (time_step * wind_acc *
np.cos(np.radians(wind_heading)))
vy = (v * np.sin(np.radians(h))) + (time_step * wind_acc *
np.sin(np.radians(wind_heading)))
x += time_step * vx
y += time_step * vy
# rF * sin(theta)
wind_torque = plane_half_length * wind_force * np.sin(
np.radians(wind_heading - h))
wind_w = 0.1 * time_step * (wind_torque /
plane_mass) * plane_half_length
v += time_step * a
h += time_step * (w + wind_w)
h %= 360
return states
def sample_environments(real_ws, real_wh):
env = [(real_ws, solver_heading(real_wh))]
for _ in range(num_samples):
env.append((np.random.randint(real_ws - ws_bins[2], real_ws + ws_bins[2] + 1),
solver_heading(np.random.randint(real_wh - wh_bins[2], real_wh + wh_bins[2] + 1))))
return env