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)