# Classes
ekf = dEKF( x, PP, Q, R )
H = array( [[1, 0, 0, 0, 0, ],
[0, 1, 0, 0, 0]] )
#Torque
torque = np.concatenate( ( np.ones( ( 1, len( sw ) / 4 ) ) * 5, np.ones( ( 1, 3 * len( sw ) / 4 ) ) * 10 ), axis = 1 );
#Simulation loop
for i in range( len( sw ) - N ):
( Ak, Bk, Ck, Dk ) = pmsm.mdss( xk, Ts )
# Calculation the optimal control signal
U = dlqr.calcU( xk, array( [sw[i:i + N]] ) , Ak = Ak, Bk = Bk, Ck = Ck )
# Voltages inverse transformation
Ulb = ct.inv_rotor_coordinate( U, P * xk[4, 0] )
U_abc[ i] = ct.inv_clarke_m_i( Ulb[0, 0], Ulb[1, 0] )[:, 0]
#Limitating the signal
for k in range( 0, U_abc.shape[1] ):
if( np.abs( U_abc[i, k] ) > 380 ):
U_abc[i, k] = np.sign( U_abc[i, k] ) * 380;
#Voltages transformation
Ulb = ct.clarke_m_i( U_abc[ i, 0], U_abc[ i, 1], U_abc[ i, 2] )
x_est = array( [[0], [0], [ 0]] ) x_est_o = array( [[0], [ 0], [ 0]] ) H = array( [[1., 0., 0.]] ) ( Ak, Bk, Ck, Dk ) = dc2.dss( Ts, "ZOH" ) N = 15 dlqr = DSTLQR( Ak, Bk, Ck, N ) kf = dKF( x_est, Ak, Bk, H ) for i in range( len( pw ) - N ): # Calculation the optimal control signal U = dlqr.calcU( x_est, array( [pw[i:i + N]] ) ) # if( abs( U ) > 220 ): # U = sign( U ) * 220; # Simulating ( sw, x ) = dc2.dlsim( U, torque[0, i ], Ts = Ts ); y_out[i, :] = sw u_out[i, :] = U # Estimating the states ( x_est, P, error ) = kf.update( x[0, 0] , U ) y_est[i, :] = x_est[1]; if( i % 1000 ) == 0: print "%d of %d" % ( i, len( pw ) )
