- added Pid

- added Temp Controller
- added Kalman

components/pid/kalman.py

@@ -0,0 +1,144 @@
+import numpy as np
+from numpy.linalg import inv
+from matplotlib.pyplot import plot, figure, subplot, title, xlabel, ylabel, grid, show
+
+
+class Kalman:
+	def __init__(self, params={}):
+		dt = params['dt']
+		var_P = params['var_P']
+		var_Q = params['var_Q']
+		var_R = params['var_R']
+		var_Z = params['var_Z']
+		model = np.matrix([1, dt, 1/2*dt**2]).transpose()
+		N = len(model)-1
+
+		P = var_P*np.eye(N)
+		R = var_R*np.eye(N)
+		H = np.eye(N)
+
+		A = np.eye(N)
+		for row in range(0, N):
+			A[row, row:N] = model.transpose()[0, 0:N-row]
+
+		G = np.matrix(model[N:0:-1])
+		Q = G * G.transpose() * var_Q
+
+		self.P = P
+		self.Q = Q
+		self.R = R
+		self.N = N
+		self.A = A
+		self.H = H
+		self.var_Z = var_Z
+
+		X = np.matrix([0, 1.0]).transpose()
+		Xp = np.matrix([0, 0]).transpose()
+		self.Xp = Xp
+		self.X = X
+
+		np.set_printoptions(precision=3)
+
+	@staticmethod
+	def print(p, d):
+		print(p)
+		print(d)
+
+	def initial(self, X):
+		self.Xp = np.matrix([X[0], X[1]]).transpose()
+
+	def process_truth(self):
+		# ----------------------------
+		# Process ground truth
+		self.X = self.A * self.X
+
+		return self.X
+
+	def process_measurement(self, y):
+
+		Y = np.matrix([y[0], y[1]]).transpose()
+		# ----------------------------
+		# Take noisy measurement
+		Z = self.H * Y + self.var_Z * np.random.randn(self.N, 1)
+
+		return Z
+
+	def process(self, Z):
+
+		# ----------------------------
+		# State estimate
+		self.Xp = self.A * self.Xp
+
+		# ----------------------------
+		# Measurement prediction
+		Zp = self.H * self.Xp
+
+		# ----------------------------
+		# Measurement residual
+		V = Z - Zp
+
+		# ----------------------------
+		# State	prediction covariance
+		self.P = self.A * self.P * self.A.transpose() + self.Q
+
+		# ----------------------------
+		# Measurement prediction covariance
+		S = self.H * self.P * self.H.transpose() + self.R
+
+		# ----------------------------
+		# Kalman gain
+		K = self.P * self.H.transpose() * inv(S)
+
+		# ----------------------------
+		# Update state estimate
+		self.Xp = self.Xp + K * V
+
+		# ----------------------------
+		# Updated state covariance
+		self.P = self.P - K * S * K
+
+		return self.Xp
+
+# Main
+if __name__ == '__main__':
+	dt =1
+
+	params = {
+		'dt'	: dt,
+		'var_P'	: 1,
+		'var_Q'	: 0,
+		'var_R'	: 1,
+		'var_Z'	: 0
+	}
+	k = Kalman(params)
+
+	_x1 = np.empty(0)
+	_y1 = np.empty(0)
+	_x2 = np.empty(0)
+	_y2 = np.empty(0)
+
+	N = int(100/dt)
+	seqn = range(0, N)
+
+	for n in seqn:
+		X = k.process_truth()
+		Z = k.process_measurement((X[0,0], 0))
+		#Kalman.print("Z:", Z)
+		Xp = k.process(Z)
+
+		_x1 = np.append(_x1, Z[0])
+		_x2 = np.append(_x2, Z[1])
+		_y1 = np.append(_y1, Xp[0])
+		_y2 = np.append(_y2, Xp[1])
+
+	figure(1)
+	subplot(2, 1, 1)
+	plot(seqn, _x1, 'bx', seqn, _y1, '-r', linewidth=1)
+	grid(True)
+	subplot(2, 1, 2)
+	plot(seqn, _x2, 'bx', seqn, _y2, '-r', linewidth=1)
+	grid(True)
+	show()
+
+	print("End of program")
+

components/pid/pid.py

@@ -0,0 +1,48 @@
+
+
+class Pid:
+    def __init__(self):
+        # Integrator
+        self.yi = 0
+
+        # Differentiator
+        self.xd = 0
+
+        # Auto windup
+        self.y_min = -1.0
+        self.y_max = 1.0
+
+        # Output
+        self.y = 0
+
+    @staticmethod
+    def params(kp, ki, kd, rho):
+        p = dict(kp=kp, ki=ki, kd=kd, rho=rho)
+        return p
+
+    def reset(self):
+        self.yi = 0
+        self.xd = 0
+
+    def process(self, dt, params, err):
+        kp = params['kp']
+        ki = params['ki']
+        kd = params['kd']
+        rho = params['rho']
+
+        yi = rho*self.yi + ki*dt * err
+        yd = err - self.xd
+
+        _yp = kp * err
+        _yi = yi
+        _yd = kd/dt * yd
+
+        y = _yp + _yi + _yd
+
+        self.y = max(self.y_min, min(self.y_max, y))
+
+        self.yi = yi
+        self.xd = err
+
+    def get_y(self):
+        return self.y

components/pid/temp_controller.py

@@ -0,0 +1,41 @@
+from components.pid import Pid
+from components.pid import Kalman
+
+
+class TempController:
+	def __init__(self, params):
+		self.dt = params['dt']
+		self.pid_hold = Pid()
+		self.pid_rate = Pid()
+		self.theta_ist = 0
+		self.theta_soll = 0
+		self.heatrate_ist = 0
+		self.heatrate_soll = 0
+		self.params = params
+
+	def set_theta_ist(self, value):
+		self.theta_ist = value
+
+	def set_heatrate_ist(self, value):
+		self.heatrate_ist = value
+
+	def set_theta_soll(self, value):
+		self.theta_soll = value
+
+	def set_heatrate_soll(self, value):
+		self.heatrate_soll = value
+
+	def process(self):
+		theta_err = self.theta_soll - self.theta_ist
+		heatrate_err = self.heatrate_soll - self.heatrate_ist
+
+		self.pid_hold.process(self.dt, self.params['Hold']['Pid'], theta_err)
+		self.pid_rate.process(self.dt, self.params['Heat']['Pid'], heatrate_err)
+
+	def get_power_hold(self):
+		power_hold = 200 + self.params['P_max'] * self.pid_hold.get_y()
+		return max(self.params['P_min'], min(self.params['P_max'], power_hold))
+
+	def get_power_heat(self):
+		power_heat = 1500 + self.params['P_max'] * self.pid_rate.get_y()
+		return max(self.params['P_min'], min(self.params['P_max'], power_heat))