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