- added Pid
- added Temp Controller - added Kalman
This commit is contained in:
jens
@@ -5,6 +5,7 @@ from ws.server.ws_server_multi_user import WsServerMultiUser
|
||||
from components.sensor.tempSensorSim import TempSensorSim, ATemperatureSensor
|
||||
from components.plant.pot import Pot, APlant
|
||||
from components.actor.heater import Heater, AHeater
|
||||
from components.pid import kalman
|
||||
from ws.message import MsgIo, MessageDispatcher, Value
|
||||
|
||||
|
||||
@@ -18,12 +19,13 @@ class ATask:
|
||||
|
||||
|
||||
class TempSensorTask(ATask):
|
||||
def __init__(self, sensor: ATemperatureSensor, interval, msg_handler: MsgIo):
|
||||
def __init__(self, sensor: ATemperatureSensor, interval, msg_handler: MsgIo, kalman: kalman.Kalman):
|
||||
ATask.__init__(self, interval)
|
||||
|
||||
self.msg_handler = msg_handler
|
||||
msg_handler.set_recv_handler(self.recv)
|
||||
self.sensor = sensor
|
||||
self.kalman = kalman
|
||||
|
||||
async def recv(self, data):
|
||||
print(data)
|
||||
@@ -35,8 +37,13 @@ class TempSensorTask(ATask):
|
||||
print("{}: Started with interval {} s".format(self.msg_handler.get_key(), self.interval))
|
||||
temp = Value()
|
||||
while True:
|
||||
temp.set(round(self.sensor.temperature(), 1))
|
||||
Z = self.kalman.process_measurement((self.sensor.temperature(), 0))
|
||||
xp = self.kalman.process(Z)
|
||||
theta_ist = xp[0, 0]
|
||||
# heatrate_ist = xp[1, 0] * 60
|
||||
temp.set(round(theta_ist, 1))
|
||||
if temp.is_changed():
|
||||
print("Sensor temp {}".format(temp.get()))
|
||||
await self.send({'Temp': temp.get()})
|
||||
await asyncio.sleep(self.interval)
|
||||
|
||||
@@ -109,42 +116,6 @@ class PotTask(ATask):
|
||||
await asyncio.sleep(self.interval)
|
||||
|
||||
|
||||
class CounterTask(ATask):
|
||||
def __init__(self, interval, msg_handler: MsgIo):
|
||||
ATask.__init__(self, interval)
|
||||
|
||||
self.msg_handler = msg_handler
|
||||
msg_handler.set_recv_handler(self.recv)
|
||||
|
||||
async def recv(self, data):
|
||||
print(data)
|
||||
|
||||
async def send(self, data):
|
||||
await self.msg_handler.send(data)
|
||||
|
||||
async def on_process(self):
|
||||
print("{}: Started with interval {} s".format(self.msg_handler.get_key(), self.interval))
|
||||
while True:
|
||||
for count in range(0, 101):
|
||||
await self.send(count)
|
||||
await asyncio.sleep(self.interval)
|
||||
|
||||
|
||||
class ColorHandler:
|
||||
def __init__(self, msg_handler: MsgIo):
|
||||
self.msg_handler = msg_handler
|
||||
msg_handler.set_recv_handler(self.recv)
|
||||
print("{}: Constructed".format(self.msg_handler.get_key()))
|
||||
|
||||
async def recv(self, data):
|
||||
print(data)
|
||||
for value in data.values():
|
||||
await self.send(value)
|
||||
|
||||
async def send(self, data):
|
||||
await self.msg_handler.send(data)
|
||||
|
||||
|
||||
class TaskManager:
|
||||
def __init__(self):
|
||||
self.tasks: ATask = []
|
||||
@@ -174,13 +145,22 @@ if __name__ == '__main__':
|
||||
dispatcher = MessageDispatcher()
|
||||
server = WsServerMultiUser(listener=dispatcher)
|
||||
taskmgr = TaskManager()
|
||||
taskmgr.add(CounterTask(1.0, dispatcher.msgio_get("a")))
|
||||
taskmgr.add(CounterTask(0.5, dispatcher.msgio_get("b")))
|
||||
taskmgr.add(CounterTask(0.1, dispatcher.msgio_get("c")))
|
||||
|
||||
# Kalman Filter
|
||||
kalman_params = {
|
||||
"dt" : 1.0,
|
||||
"var_P" : 0,
|
||||
"var_Q" : 0.000001,
|
||||
"var_R" : 1,
|
||||
"var_Z" : 0.0
|
||||
}
|
||||
kalman = kalman.Kalman(kalman_params)
|
||||
|
||||
# Sensor
|
||||
sensor = TempSensorSim()
|
||||
taskmgr.add(TempSensorTask(sensor, 1.0, dispatcher.msgio_get("Sensor")))
|
||||
taskmgr.add(TempSensorTask(sensor, 1.0, dispatcher.msgio_get("Sensor"), kalman))
|
||||
|
||||
# Plant
|
||||
pot_params = {
|
||||
"dt" : 0.1,
|
||||
"theta_amb" : 15,
|
||||
@@ -190,8 +170,6 @@ if __name__ == '__main__':
|
||||
"Td" : 12,
|
||||
"kn" : 0.2
|
||||
}
|
||||
# {"Heater": {"Activate": 1, "Power": 1000}}
|
||||
# Plant
|
||||
pot = Pot(pot_params)
|
||||
pot.connect_theta_changed(sensor.set_fake_temp)
|
||||
taskmgr.add(PotTask(pot, 0.1, dispatcher.msgio_get("Pot")))
|
||||
@@ -201,8 +179,6 @@ if __name__ == '__main__':
|
||||
heater.connect_power_changed(pot.setPower)
|
||||
taskmgr.add(HeaterTask(heater, 0.1, dispatcher.msgio_get("Heater")))
|
||||
|
||||
ColorHandler(dispatcher.msgio_get("Color"))
|
||||
|
||||
h_dispatcher = taskmgr.start()
|
||||
h_server = server.listen("localhost", 8765)
|
||||
asyncio.gather(h_dispatcher, h_server)
|
||||
|
||||
@@ -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")
|
||||
|
||||
@@ -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
|
||||
@@ -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))
|
||||
|
||||
Reference in New Issue
Block a user