self.P = self.F * self.P * np.transpose(self.F)
return
def measure_and_update(self, measurements, t):
dt = t - self.prev_time
for i in range((np.shape(self.F)[0] // 2)):
self.F[i, i + 2] = dt
Z = np.matrix(measurements)
y = np.transpose(Z) - self.H * self.x
S = self.H * self.P * np.transpose(self.H) + self.R
K = self.P * np.transpose(self.H) * np.linalg.inv(S)
self.x = self.x + K * y
self.P = (self.I - K * self.H) * self.P
for i in range(np.shape(self.P)[0]):
self.P[i, i] += 1
#self.P[1,1] += 100
#self.P[2,2]+= 100
#self.P[3,3] += 100
self.v = (self.x[1, 0])
self.prev_time = t
return [self.x[0], self.x[1]]
def recieve_inputs(self, u_steer, u_pedal):
return
sim_run(options, KalmanFilter)
v_t_1 = v_t + a_t * dt - v_t / 25
return [x_t_1, y_t_1, psi_t_1, v_t_1, a_t, phi_t]
def cost_function(self, u, *args):
state = args[0]
ref = args[1]
cost = 0.0
for k in range(0, self.horizon):
state = self.plant_model(state, self.dt, u[k * 2], u[k * 2 + 1])
# Cost function tuned to obtain optimized control action
cost += abs(ref[0] -
state[0])**2 + abs(ref[1] - state[1])**2 + abs(
ref[2] - state[2])**2 + self.obstacle_distance(
state[0], state[1])
return cost
# Obstacle avoidance
def obstacle_distance(self, x, y):
distance = ((self.x_obs - x)**2 + (self.y_obs - y)**2)**0.5
if (distance > 2):
return 15
else:
return 1 / distance * 30
sim_run(options, ModelPredictiveControl)