Python線形化倒立振子のプログラムを動かしたい.

前提

倒立振子の線形化下プログラムを作成しましたが、エラーが解決できませんでした.

実現したいこと

シミュレーションをすること.
グラフが発散することを確認できるようにする.

発生している問題・エラーメッセージ

can't multiply sequence by non-int of type 'float'

該当のソースコード

Python
1
2state_dx = dynamicalsystem(state_x[:, counter-1], input_u[:, counter-1])
3
4ソースコードは下のようになりました.

!pip install control
!pip install --upgrade cvxpy

%reset -f
import numpy as np
import scipy as sp
import matplotlib.pyplot as plt

#パラメータ
g=9.81
m=0.0467
R_W=0.066
J_W=(mR_WR_W)/2
M=0.806
L=0.75689
J_theta=(MLL)/3
J_m=0.00001
R_m=6.69
K_t=0.317
f_m=0.0022
f_W=0
K_b=0.468
theta = 30;
h = 0.001
#線形係数
A1=-R_m*(MLL+J_theta+J_m)/K_t
A2=-R_m*(-J_m+MR_WL)/K_t
A3=(R_mMgL)/K_t
A4=(-K_tK_b-f_mR_m)/K_t
A5=K_b+(R_mf_m)/K_t
B1=R_m*(MR_WR_W+mR_WR_W+J_W+J_m)/K_t
B2=R_m*(MR_WL-J_m)/K_t
B3=(K_tK_b+R_mf_m)/K_t
B4=(-K_tK_b-R_mf_m-R_mf_W)/K_t
AA1= (A1B4-B2A4)/(B2A2-B1A1)
AA2 =(-B2A3)/(B2A2-B1A1)
AA3 =(A1B3-B2A4)/(B2A2-B1A1)
BB1= (A2B3-B1A5)/(B1A1-B2A2)
BB2 =(-B1A3)/(B1A1-B2A2)
BB3 =(A2B3-B1A4)/(B1A1-B2A2)
CC1 = (B2-A1)/ (B2A2-B1A1)
CC2 = (B1-A2)/ (B1A1-B2*A2)

def dynamicalsystem(state,input):
derivative = np.matrix(np.zeros((SIZE_STATE,1)))
derivative[0, 0] = state[1, 0]
derivative[1, 0] = np.vstack( [AA1state[1, 0] + AA2state[2, 0] + AA3state[3, 0] + CC1input[0,0]])
derivative[2, 0] = state[3,0]
derivative[3, 0] =np.vstack( [BB1state[1,0] + BB2state[2,0] + BB3*[3,0] + CC2input[0,0]]) return np.vstack([ state[1, 0] , (AA1state[1, 0] + AA2state[2, 0] + AA3state[3, 0] + CC1input[0,0]) , state[3,0] , (BB1state[1,0] + BB2state[2,0] + BB3[3,0] + CC2*input[0,0]) ])
return np.vstack([ state[1, 0] , (AA1*state[1, 0] + AA2*state[2, 0] + AA3*state[3, 0] + CC1*input[0,0]) , state[3,0] , (BB1*state[1,0] + BB2*state[2,0] + BB3*[3,0] + CC2*input[0,0]) ])  
 

def controller(state, reference):
return np.vstack( (0.0,0.0,0.0, 0.0) )

NUMBER_STEPS = int(10000) #NUMBER_STEPSに整数の10000配列格納
TIME_START = 0.0
TIME_END = 10.0
TIME_STEP = (TIME_END - TIME_START)/float(NUMBER_STEPS)
SIZE_INPUT = int(1)
SIZE_OUTPUT = int(1)
SIZE_STATE = int(4)
time_t = np.matrix(np.zeros((1, NUMBER_STEPS)))
input_u = np.matrix(np.zeros((SIZE_INPUT, NUMBER_STEPS)))
output_y = np.matrix(np.zeros((SIZE_OUTPUT, NUMBER_STEPS)))
state_x = np.matrix(np.zeros((SIZE_STATE, NUMBER_STEPS)))
reference_r = np.matrix(np.zeros((SIZE_OUTPUT, NUMBER_STEPS)))

state_dx = np.matrix(np.zeros((SIZE_STATE, 1)))

GAIN_K = np.matrix([[1.1, 0.1]])
STATE_INTTAL = np.matrix(( [2], [0],[-1],[2]))

state_x[:, 0] = STATE_INTTAL
input_u[:, 0] = np.matrix(( [0]))
state_dx = np.matrix(np.zeros((SIZE_STATE, 1)))
for counter in range(1, NUMBER_STEPS):
time_t[:, counter] = TIME_STEPcounter
input_u[:, counter-1] = np.matrix(( [0.0]))
state_dx = dynamicalsystem(state_x[:, counter-1], input_u[:, counter-1])
state_x[:, counter] = state_x[:, counter-1] + state_dxTIME_STEP
plt.plot(np.ravel(time_t), np.ravel(state_x[0, :]))
plt.show()
plt.plot(np.ravel(time_t), np.ravel(state_x[1, :]))
plt.show()
plt.plot(np.ravel(time_t), np.ravel(state_x[2, :]))
plt.show()
plt.plot(np.ravel(time_t), np.ravel(state_x[3, :]))
plt.show()