def integrate(self, n, state, dt):
pid = PID(kp=self.kp, ki=self.ki, kd=self.kd, angle=self.pidAngle, dt=dt)
history = np.array([state])
theta = Robot._get_gyroscope(state[2])
for i in range(n):
self.u = pid.execute_pid(theta)
state = rk4(self.dynamics, state, i, dt)
theta = Robot._get_gyroscope(Robot._get_gyroscope(state[2]))
history = np.append(history, [state], axis=0)
return history
def execute_kalman_filter(self, n, state, dt):
pid = PID(kp=self.kp, ki=self.ki, kd=self.kd, angle=self.pidAngle, dt=dt)
history = np.array([state])
theta = state[2]
x = state
h = np.matrix([
[1, 0, 0, 0],
[0, 1, 0, 0]
])
p = np.matrix([
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0]
])
q = np.matrix([
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
r = np.matrix([
[10**-3, 0],
[0, 10**-3]
])
for i in range(n):
z = (h * x) + np.matrix([[np.random.randn()*0.001], [np.random.randn()*0.001]])
u = pid.execute_pid(theta)
x = (self.A * x) + (self.B * u)
p = (self.A * p * self.A.T) + q
y = z - (h * x)
s = (h * p * h.T) + r
k = p * h.T * np.linalg.inv(s)
x = x + (k * y)
p = (np.eye(4) - (k * h))*p
theta = x[2]
history = np.append(history, [x], axis=0)
return history
