from scipy import * from scipy.signal import * from tools.signalgenerator import sinwave import random from model.dcmotor import dcmotor from estimation.KalmanFilter import dKF import pylab """ @summary: A simple test of the Extended Kalman Filter """ if __name__ == "__main__": dc2 = dcmotor() Ts = 0.0001 ekf = dKF(x0=zeros((3, 1))) mtime = arange(0, 0.01, Ts) yo = zeros((len(mtime), 1)) xk = zeros((len(mtime), 3)) xe = zeros((len(mtime), 3)) noise = zeros((len(mtime), 3)) error = zeros((len(mtime), 3)) (A, B, C, D) = dc2.dss(Ts) sinW = sinwave(Ts=Ts, mtime=0.01, freq=100, amp=20) pw = sinW.signal() H = array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) for i in range(len(mtime)):
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 )