#Kalman filter covariance matrices
Q = np.identity( 5 )
PP = 1e-3 * np.identity( 5 )
Q[0][0] = 1e-4
Q[1][1] = 1e-4
Q[2][2] = 1e-4
Q[3][3] = 1e-9
Q[4][4] = 1e-4
R = 1e-3 * np.identity( 2 )
# 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 )
PP = 1e-3 * identity(5)
Q = identity(5)
"""ri = random.uniform( 1e-7, 1e-2, )
fi = random.uniform( 1e-7, 1e-2 )
wi = random.uniform( 1e-7, 1e-2 )"""
Q[0][0] = 1e-4
Q[1][1] = 1e-4
Q[2][2] = 3e-2
Q[3][3] = 3e-2
Q[4][4] = 1e-8
#
R = 1e-7 * identity(2)
# Classes
ekf = dEKF(xk, PP, Q, R)
w_r = zeros((len(time), 3))
for i in range(0, int(len(time) / 5)):
(Ak, Bk, H, D) = acim.dss_statorreferenceframe1(xk[4], Ts)
Ulb = coord_trans.clarke_m_i(voltage[i][0], voltage[i][1], voltage[i][2])
Ilb = coord_trans.clarke_m_i(current[i][0], current[i][1], current[i][2])
Udq = coord_trans.stator_coordinate(Ulb, teta[i][0])
Idq = coord_trans.stator_coordinate(Ilb, teta[i][0])
xk = ekf.update_state(Udq, Ak, Bk);
Gk = acim.dss_Gk_stator1(xk[2], xk[3], xk[4], Ts)
(xk, PP, error) = ekf.update(Idq, Udq, Gk, Ak, Bk, H)
w_r[i][0] = xk[ 4 ][0]
w_r[i][1] = speed[i][0]
w_r[i][2] = speed[i][1]
Q = identity( 6 )
"""ri = random.uniform( 1e-7, 1e-2, )
fi = random.uniform( 1e-7, 1e-2 )
wi = random.uniform( 1e-7, 1e-2 )"""
Q[0][0] = 1e-2
Q[1][1] = 1e-2
Q[2][2] = 1e-4
Q[3][3] = 1e-4
Q[4][4] = 1e-6
Q[5][5] = 1e-6
#
R = 1e-7 * identity( 2 )
#Classes
ekf = dEKF( xk, PP, Q, R )
w_r = zeros( ( len( time ), 3 ) )
for i in range( 0, int( len( time ) / 5 ) ):
( Ak, Bk, H, D ) = acim.dss_statorreferenceframe2( xk[4], Ts )
Ulb = coord_trans.clarke_m_i( voltage[i][0], voltage[i][1], voltage[i][2] )
Ilb = coord_trans.clarke_m_i( current[i][0], current[i][1], current[i][2] )
Udq = coord_trans.stator_coordinate( Ulb, teta[i][0] )
Idq = coord_trans.stator_coordinate( Ilb, teta[i][0] )
xk = ekf.update_state( Udq, Ak, Bk );
Gk = acim.dss_Gk_stator1( xk[2], xk[3], xk[4], Ts )
( xk, PP, error ) = ekf.update( Idq, Udq, Gk, Ak, Bk, H )
w_r[i][0] = xk[ 4 ][0]
w_r[i][1] = speed[i][0]
w_r[i][2] = speed[i][1]