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