# Identity Matrix self.I = np.matrix([[1., 0.], [0., 1.]]) def predict(self, t): # Calculate dt. dt = t - self.prev_time # Put dt into the state transition matrix. self.F[0, 1] = dt self.x = self.F * self.x self.P = self.F * self.P * np.transpose(self.F) return self.x[0, 0] def measure_and_update(self, measurements, t): dt = t - self.prev_time self.F[0, 1] = 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 self.P[0, 0] += 0.1 self.P[1, 1] += 0.1 self.v = self.x[1, 0] self.prev_time = t return sim_run(options, KalmanFilter)
import numpy as np from sim.sim1d import sim_run # Simulator options. options = {} options['FIG_SIZE'] = [8,8] options['CONSTANT_SPEED'] = False class KalmanFilterToy: def __init__(self): self.v = 0 self.prev_x = 0 self.prev_t = 0 def predict(self,t): prediction = self.v * (t-self.prev_t) + self.prev_x #predict the x value return prediction def measure_and_update(self,x,t): measured_v = (x-self.prev_x)/(t-self.prev_t) self.v += (measured_v - self.v)*0.4 #higher value => more responsive, more wavy #lower value less weight on recent measurement, more accurate self.prev_x = x self.prev_t = t return sim_run(options,KalmanFilterToy)
class ModelPredictiveControl: def __init__(self): self.horizon = 20 self.dt = 0.2 # Reference or set point the controller will achieve. self.reference = [50, 0, 0] def plant_model(self, prev_state, dt, pedal, steering): x_t = prev_state[0] v_t = prev_state[3] # m/s a_t = pedal x_t_1 = x_t + v_t * dt v_t_1 = v_t + a_t * dt - v_t/25 return [x_t_1, 0, 0, v_t_1] def cost_function(self,u, *args): state = args[0] ref = args[1] cost = 0.0 for i in range(0, self.horizon): state = self.plant_model(state, self.dt, u[i*2], 0) cost += (ref[0] - state[0]) ** 2 # de 10 km/h a m/s if abs(state[3]) > 10 * (1000/3600): cost += 100 ** (state[3]) return cost sim_run(options, ModelPredictiveControl)