Fixing bugs in StateSpace in Block.py

notebooks/Pysim/Block.py

@@ -416,24 +416,16 @@ class StateSpace(Block):
         self.ninput = len(inp)
         self.noutput = len(out)
         self.nstate = len(A)
-        self.A = numpy.zeros((self.nstate,self.nstate))
-        self.B = numpy.zeros((self.nstate,self.ninput))
-        self.C = numpy.zeros((self.noutput,self.nstate))
-        self.D = numpy.zeros((self.noutput,self.ninput)) 
         self.A = A
         self.B = B
         self.C = C
         self.D = D
-        self.inpos = numpy.zeros((self.ninput),dtype=numpy.int)
-        self.outpos = numpy.zeros((self.noutput),dtype=numpy.int)
+        self.inpos = inp
+        self.outpos = out
         self.x = numpy.zeros((self.nstate))
         self.xo = numpy.zeros((self.nstate))
         self.u = numpy.zeros((self.ninput))
         self.y = numpy.zeros((self.noutput)) 
-        for i in range(0,self.ninput):
-            self.inpos[i] = inp[i]
-        for i in range(0,self.noutput):
-            self.outpos[i] = out[i]
         for i in range(0,self.nstate):
             self.x[i] = xo[i]
             self.xo[i] = xo[i]
@@ -451,9 +443,9 @@ class StateSpace(Block):
                 else:
                     print("WARNING: Can not handle self.C with dimension "+str(self.C.ndim))
             for j in range(0,self.ninput):
-                if self.C.ndim == 1:
+                if self.D.ndim == 1:
                     self.y[i] = self.y[i] + self.D[j]*self.u[j]
-                elif self.C.ndim == 2:
+                elif self.D.ndim == 2:
                     #prevent [ValueError: setting an array element with a sequence.] for 2 dimensional self.C:
                     self.y[i] = self.y[i] + self.D[i][j]*self.u[j]
                 else:
@@ -465,12 +457,7 @@ class StateSpace(Block):
             self.x[i] = xo[i]
                                    
     def cdxdt(self,x,t):
-        dxdt = numpy.zeros((self.nstate)) 
-        for i in range (0,self.nstate):
-            for j in range (0,self.ninput):
-                dxdt[i] = self.B[i,j]*self.u[j]
-            for j in range (0,self.nstate):
-                dxdt[i] = dxdt[i] + self.A[i,j]*x[j]
+        dxdt = numpy.matmul(self.B,self.u.T) + numpy.matmul(self.A,self.x)
         return dxdt