# # pylab.show() #Control signals # squareW = squarewave( Ts = Ts, mtime = 10 , freq = 0.2, amp = 30 ) # sw = squareW.signal() sinW = sinwave( Ts = Ts, mtime = 1, freq = 2, amp = 30 ) sw = sinW.signal() ct = CT() #Number of horizon N = 2 ( Ak, Bk, Ck, Dk ) = pmsm.mdss( x, Ts ) dlqr = DSTLQR( Ak, Bk, Ck, N ) w_s = zeros( ( len( sw ), 1 ) ) U_s = zeros( ( len( sw ), 2 ) ) x_s = zeros( ( len( sw ), 5 ) ) error = zeros( ( len( sw ), 1 ) ) w_est = zeros( ( len( sw ), 1 ) ) O_est = zeros( ( len( sw ), 1 ) ) U_abc = zeros( ( len( sw ), 3 ) ) O = zeros( ( len( sw ), 1 ) ) #Kalman filter covariance matrices Q = np.identity( 5 ) PP = 1e-3 * np.identity( 5 ) Q[0][0] = 1e-4
dc2.x0 = None; y_out = zeros( ( len( pw ), 1 ) ) y_est = zeros( ( len( pw ), 1 ) ) u_out = zeros( ( len( pw ), 1 ) ) 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 )