# 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 ) )