Add files and make changes for notebooks/V10...

notebooks/Pysim/Block.py

 import numpy
 sin = numpy.sin
+from random import triangular
 
 #########################################################################
 #### Abstract class for continuous time block in a block diagram ########
@@ -95,6 +96,49 @@ class SinusoidalSignalSource(Block):
             self.y[0] = self.Am*sin(self.om*t+self.phi)    
             
 
+#########################################################################
+########################### Unknown wave source #########################
+#########################################################################
+            
+class UnknownWaveSource(Block):
+    def __init__(self,out,startv,Am,om,phi):
+            super(UnknownWaveSource, self).__init__()
+            self.noutput = 1
+            self.outpos = numpy.zeros((self.noutput),dtype=numpy.int)
+            self.outpos[0] = out
+            self.Am = Am
+            self.om = om
+            self.phi = phi
+            self.y = numpy.zeros((self.noutput))   
+            self.y[0] = startv
+                    
+    def Step(self,t,dt):
+            #Alternativ: self.y[0] = self.Am*sin(otp)+0.1*self.Am*sin(otp*0.1)
+            opt = (self.om*t)+self.phi
+            self.y[0] = self.Am*(sin(opt)-0.3*(sin(opt))**2+0.2*(sin(opt-1))**2)
+
+#########################################################################
+################################## Noise ################################
+#########################################################################
+            
+class Noise(Block):
+    def __init__(self,inp,out,Am):
+            super(Noise, self).__init__()
+            self.noutput = 1
+            self.outpos = numpy.zeros((self.noutput),dtype=numpy.int)
+            self.outpos[0] = out
+            self.ninput = 1
+            self.inpos = numpy.zeros((self.ninput),dtype=numpy.int)   
+            self.inpos[0] = inp
+            self.Am = Am
+            self.u = numpy.zeros((self.ninput))
+            self.y = numpy.zeros((self.noutput))   
+            #self.y[0] = startv
+                    
+    def Step(self,t,dt):
+            noise = ([self.Am*triangular(0.0,0.5,1.0)-(self.Am/2.0)])
+            self.y = self.u + numpy.array(noise)
+
 #########################################################################
 ######################### Square signal source ##########################
 #########################################################################

notebooks/Pysim/DTBlock.py

 import numpy
 from Block import Block
+inv = numpy.linalg.inv
+dot = numpy.dot
 
 #########################################################################
 #### Abstract class for discrete time block in a block diagram ########
@@ -317,3 +319,77 @@ class DTStateSpace(DTBlock):
         for i in range(0,self.nstate):
             self.x[i] = 0
 
+
+
+
+#########################################################################
+########################### Kalman Filter  ##############################
+#########################################################################
+
+class KalmanFilter(DTBlock):
+    def __init__(self,inp,out,nin,nmeas,F,G,C,Q,R,xo,Pk,Ts):
+        self.F = F
+        self.G = G
+        self.C = C
+        self.Q = Q
+        self.R = R
+        self.x = xo
+        self.Pk = Pk
+        self.Ts = Ts
+        self.nin = nin
+        self.nmeas = nmeas
+        self.ninput = len(inp)
+        self.noutput = len(out)
+        self.inpos = inp;
+        self.outpos = out;
+        self.nstate = len(F);
+        self.u = numpy.zeros((self.ninput))
+        self.y = numpy.zeros((self.noutput))
+        self.lastt = -10
+            
+        
+    def updatediscrete(self):
+        
+        uu = numpy.zeros((self.nstate))
+        m = numpy.zeros((self.nstate))
+        if (self.nin != 0):
+            for i in range(0,self.nin):
+                uu[i] = self.u[i];
+        for i in range(0,self.nmeas):
+            m[i] = self.u[self.nin+i]
+
+        # Zustandsvorhersage:
+        # x = F*x + G*u
+        self.x = dot(self.F,self.x)+dot(self.G,uu)
+        # Kovarianz der Zustandsvorhersage:
+        # Pk = F*P*F' + Q
+        self.Pk = dot(self.F,dot(self.Pk,self.F.T)) + self.Q
+
+        # Kovarianz der Innovation:
+        # S = C*Pk*C' + R
+        S = dot(self.C, dot(self.Pk, self.C.T))+ self.R
+
+        # Kalman Gain:
+        # Kk = Pk*self.C.T*inv(S)
+        Kk = dot(dot(self.Pk, self.C.T), inv(S))
+
+        # Messvorhersage:
+        # z = C*x
+        z = dot(self.C,self.x)
+        
+        # Innovation:
+        v = m - z
+        
+        # Kovarianzkorrektur:
+        # Pk = (I-(Kk*C))*Pk
+        self.Pk= dot((numpy.eye(self.nstate)-dot(Kk,self.C)),self.Pk)
+        
+        # Zustandskorrektur:
+        self.x = self.x+dot(Kk,v)
+        
+        # Widergabe des Zustandsvektors:
+        self.y = self.x
+
+    def Reset(self):
+        self.x = xo
+        self.Pk = Pk

notebooks/systheo2functions.py

@@ -4,6 +4,17 @@ from scipy import signal
 from scipy import linalg
 from itertools import zip_longest
 import matplotlib.pyplot as plt
+import sys
+sys.path.append('Pysim')
+from Schema import Schema
+from Block import (Constant, StateSpace, Gain, StepSource,
+Division, Product, Saturation, PI,PID, Sum, SquareSignal,
+SinusoidalSignalSource, Integrator, Not, TransferFunction,
+UnknownWaveSource, Noise)
+from DTBlock import (ZOH, DTStateSpace, FIR, DTIntegral,
+                     DTDelay, IIR, DTTransferFunction, KalmanFilter)
+import ipywidgets as widgets
+from ipywidgets import interact
 inv = np.linalg.inv
 pi = np.pi
 sin = np.sin
@@ -12,6 +23,7 @@ sqrt = np.sqrt
 exp = np.exp
 atan2 = np.arctan2
 log10 = np.log10
+dot = np.dot
 
 s = control.TransferFunction.s
 
@@ -92,14 +104,3 @@ def margin(G):
     else:
         print("Amplitudenreserve: "+str(np.around(gm,3))+" bei Frequenz: "+str(np.around(wg,3))+"rad/s")
 
-import sys
-sys.path.append('Pysim')
-from Schema import Schema
-from Block import (Constant, StateSpace, Gain, StepSource,
-Division, Product, Saturation, PI,PID, Sum, SquareSignal,
-SinusoidalSignalSource, Integrator, Not, TransferFunction)
-from DTBlock import ZOH, DTStateSpace, FIR, DTIntegral, DTDelay, IIR, DTTransferFunction
-
-import ipywidgets as widgets
-from ipywidgets import interact
-