在运行离散时间系统的模拟时，我无法获得原始矩阵方程的等效标量表达式以生成相同的输出（我需要将所有矩阵表达式转换为标量以便实现代码我的嵌入式C项目。）
请使用以下输入参数运行该功能。
torque_sim_ctest(0.006,0,0.01,12)
单击超链接以查看使用矩阵表达式（correct）计算的预期输出的屏幕截图与使用（假设的）等效标量表达式（incorrect）的输出。
下面是代码，默认情况下应该提供correct输出。要测试标量计算，请注释第98行（qo_next = np.dot(Ado,qo) + np.dot(Bo,np.vstack([hallf, v[:,k]]))），取消注释100到102，运行脚本，然后使用上面显示的相同参数调用该函数。
与上面突出显示的qo_next = ...矩阵表达式串联取消注释的行：
# qo_next[0] = Ado[0,0]*qo[0] + Ado[0,1]*qo[1] + Ado[0,2]*qo[2] + Bo[0,0]*hallf
# qo_next[1] = Ado[1,0]*qo[0] + Ado[1,1]*qo[1] + Ado[1,2]*qo[2] + Bo[1,0]*hallf + Bo[1,1]*v[:,k]
# qo_next[2] = Ado[2,0]*qo[0] + Ado[2,1]*qo[1] + Ado[2,2]*qo[2] + Bo[2,0]*hallf
如果有人能够指出代码的标量版本中的错误并帮我修复这个（数字？）问题，我真的很感激。谢谢！
import numpy as np
import scipy.signal as sig
import matplotlib.pyplot as plt
def torque_sim_ctest(K,Kp,Kd,tau_ampl):
# Example parameter values: torque_sim_ctest(0.006,0,0.01,12)
# -----------------------------------------------------------------------------
# Motor & System Parameters in continuous time:
bm, Jm, K_e, K_t = 1.5e-6, 3.507e-6, 12.5e-3, 12.5e-3
R, L = 0.476, 0.334e-3
gr, Jl, bl = 16.348, 10, 20
Jeq, beq = Jl + Jm*gr**2, bl + bm*gr**2
A = np.array([ [ 0, 1 ],
[ 0, -beq/Jeq ] ])
Ac = np.array( [[ 0, 1, 0 ],
[ 0, 0, 1 ],
[ 0, 0, 0 ]])
B = np.array([ [ 0, 0 ],
[ gr/Jeq, gr**2*K_t/(Jeq*R) ]])
Bc = np.array([ 0, 1, 0 ]).reshape(3,1)
C = np.array([ 1, 0 ])
Co = np.array([ 1, 0, 0 ]).reshape(1,3)
# -----------------------------------------------------------------------------
# Controller update interval Tc and simulation time step Td
Tc = 0.01
Td = 0.1*Tc
# Now discretise the continuous-time system model
Ad, Bd, Cd, Dd, dt = sig.cont2discrete((A,B,C,0),Td,method='bilinear')
# Output smoothing: filter parameters
f_ord = 2
num, den = sig.cheby1(f_ord,5,2*np.pi*10,'low',analog=True)
Af, Bf, Cf, Df = sig.tf2ss(num,den)
Afd, Bfd, Cfd, Dfd, dt = sig.cont2discrete((Af,Bf,Cf,Df),Tc,
method='bilinear')
# -----------------------------------------------------------------------------
# Observer Parameters
eps = 0.01
alph = Tc/eps
a1, a2, a3 = 3, 3, 1
Ho = np.array([a1, a2, a3]).reshape(3,1)
Ao = Ac - np.dot(Ho,Co)
D = np.diag([1, eps, eps**2])
Do = np.zeros([3,1])
Ado, Bdo, Cdo, Ddo, dt = sig.cont2discrete((Ao,Ho,np.linalg.inv(D),Do),
alph,method='bilinear')
Bo = np.hstack([Bdo.reshape(3,1), Bc.reshape(3,1)])
# -----------------------------------------------------------------------------
# Total simulation steps
n = 5001
# Define initial conditions prior to running simulation
x, q = np.zeros([2,n]), np.zeros([2,1])
q_next = np.zeros([2,1])
y, v = 0, np.zeros([1,n])
tau_ext = np.zeros([1,n])
hallc = 0
qf, qf_next, hallf = np.zeros([f_ord,1]), np.zeros([f_ord,1]), 0
qo, qo_next, xhat = np.zeros([3,1]), np.zeros([3,1]), np.zeros([3,1])
# The for loop below simulates the closed-loop system in discrete time steps Td
print('Simulation running; please wait ...')
for k in range(n-1):
# External torque input with peaks at 0.5 s and 1.6 s
tau_ext[:,k] = tau_ampl*(0.5*np.exp(-10*(k*Td-0.5)**2) \
+ np.exp(-5*(k*Td-1.6)**2))
# y is the output angle in SI units (radians)
y = gr*np.dot(C,x[:,k])
# Conversion of rotor angle to ideal hall-count
hallc = int(150/np.pi*y+0.5*np.sign(y))
# Now apply a smoothing filter
# qf_next = Afd @ qf + Bfd @ np.matrix([hallc[:,k]/gr])
# hallf = Cf @ (qf + qf_next)
qf_next[0] = 0.661939208333454*qf[0] - 19.836230603818*qf[1] + 0.00830969604166727*hallc/gr
qf_next[1] = 0.00830969604166727*qf[0] + 0.90081884698091*qf[1] + 4.15484802083364e-5*hallc/gr
# Filtered hall-count
hallf = 1342.37547903*(qf[1] + qf_next[1])
# -----------------------------------------------------------------------------
# State Observer & Control Law
# Update the controller states only every Tc = 10*Td time step
if np.mod(k,int(Tc/Td+0.5)) == 0:
qo_next = np.dot(Ado,qo) + np.dot(Bo,np.vstack([hallf, v[:,k]]))
# qo_next[0] = Ado[0,0]*qo[0] + Ado[0,1]*qo[1] + Ado[0,2]*qo[2] + Bo[0,0]*hallf
# qo_next[1] = Ado[1,0]*qo[0] + Ado[1,1]*qo[1] + Ado[1,2]*qo[2] + Bo[1,0]*hallf + Bo[1,1]*v[:,k]
# qo_next[2] = Ado[2,0]*qo[0] + Ado[2,1]*qo[1] + Ado[2,2]*qo[2] + Bo[2,0]*hallf
# qo_next[0] = 1.40740740740741*hallf - 0.407407407407407*qo[0] + 0.296296296296296*qo[1] + 0.148148148148148*qo[2]
# qo_next[1] = 1.03703703703704*hallf - 1.03703703703704*qo[0] + 0.481481481481481*qo[1] + 0.740740740740741*qo[2] + v[:,k]
# qo_next[2] = 0.296296296296296*hallf - 0.296296296296296*qo[0] - 0.148148148148148*qo[1] + 0.925925925925926*qo[2]
xhat[0] = 0.703703703703704*hallf + 0.296296296296296*qo[0] + 0.148148148148148*qo[1] + 0.0740740740740741*qo[2]
xhat[1] = 51.8518518518519*hallf - 51.8518518518518*qo[0] + 74.0740740740741*qo[1] + 37.037037037037*qo[2]
xhat[2] = 1481.48148148148*hallf - 1481.48148148148*qo[0] - 740.740740740741*qo[1] + 9629.62962962963*qo[2]
# xhat = Cdo @ qo + Ddo @ np.matrix(hallf)
# Update the control voltage v[:,k]
v[:,k] = K*xhat[2] + Kp*hallf + Kd*xhat[1]
if v[:,k] < 0:
v[:,k] = 0
elif v[:,k] >= 12:
v[:,k] = 12
else:
# Wait and hold; don't update in this cycle
qo_next = qo
v[:,k] = v[:,k-1]
# -----------------------------------------------------------------------------
# Now calculate the system dynamics in discrete time steps Td
q_next = np.dot(Ad,q) + np.dot(Bd,np.vstack([tau_ext[:,k], v[:,k]]))
x[:,k+1:k+2] = q + q_next
q = q_next
qf[0] = qf_next[0]
qf[1] = qf_next[1]
qo[0] = qo_next[0]
qo[1] = qo_next[1]
qo[2] = qo_next[2]
# ** End For Loop **
# Plot the simulation output
# NOTE: System velocity signal is x[1,:], and position is y = x[0,:]
print('Done.')
tau_spr = gr*K_t/R*v[0,0:n-1]
tau_net = tau_spr + tau_ext[0,0:n-1]
t = np.linspace(0.0,(n-1)*Td,num=n-1)
fig1, ax1 = plt.subplots(2,2)
ax1[0,0].plot(t,tau_ext[0,0:n-1],'r--',t,tau_spr,'b-')
ax1[0,0].set_xlabel('Time (s)')
ax1[0,0].set_ylabel('Torque (N m)')
ax1[0,0].set_title('Torque Comparison')
ax1[0,1].plot(t,tau_ext[0,0:n-1]/tau_net,'r--',t,tau_spr/tau_net,'b-')
ax1[0,1].set_xlabel('Time (s)')
ax1[0,1].set_ylabel('Torque (Normalised)')
ax1[0,1].set_title('Torque Comparison')
ax1[1,0].step(t,v[0,0:n-1],'g-')
ax1[1,0].set_xlabel('Time (s)')
ax1[1,0].set_ylabel('Drive Voltage (V)')
ax1[1,0].set_title('Motor Voltage')
ax1[1,1].step(t,30/np.pi*x[1,0:n-1],'m-')
ax1[1,1].set_xlabel('Time (s)')
ax1[1,1].set_ylabel('Motor Speed (rpm)')
ax1[1,1].set_title('Motor Speed vs Time')
答案 0 :(得分：0)
当我使用向量计算时：qo_next = np.dot(Ado,qo) + np.dot(Bo,np.vstack([hallf, v[:,k]]))，然后我进一步向下执行Zero-Order-Hold：qo_next = qo似乎可以按预期工作。
然而，当我将第一个赋值转换为一组标量表达式时，似乎我必须也执行逐个元素的赋值来实现Z.O.H.，即：
qo_next[0] = qo[0]
qo_next[1] = qo[1]
qo_next[2] = qo[2]
当我这样做时，标量转换神奇地起作用并产生相当于矢量版本的输出。
这真是令人头疼 - 也许有人可以向我解释一下如何在Python中进行分配，这样我就可以理解这一点。谢谢你的阅读。