# 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)
