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