def KF(M,x0,u,y,th=[],Q=[],R=[]):
#if M.typ != 'ltiss':
# raise Exception('KF code for non-linear state-space (EKF) not complete');
if isempty(th):
th = M.th;
if isempty(Q):
Q = M.Q;
if isempty(R):
R = M.R;
A = M.A(zeros(M.nx),zeros(M.nu),th);
C = M.C(zeros(M.nx),zeros(M.nu),th);
#K,P,S = dare(A,C,Q,R);
nx = M.nx;
ny = M.ny;
N = len(u[0,:]);
K = zeros((nx,ny));
P = zeros((nx,nx));
S = zeros((ny,ny));
xp = zeros((nx,N)); xp[:,0] = x0;
xu = zeros((nx,N)); xu[:,0] = x0;
yp = zeros((ny,N)); yp[:,0] = M.h(xp[:,0],u[:,0],th);
yu = zeros((ny,N)); yu[:,0] = M.h(xu[:,0],u[:,0],th);
for k in range(1,N):
A[:,:] = M.A(xu[:,k-1],u[:,k-1],th);
C[:,:] = M.C(xu[:,k-1],u[:,k-1],th);
# Predict
xp[:,k] = M.f(xu[:,k-1],u[:,k-1],th);
yp[:,k] = M.h(xp[:,k ],u[:,k ],th);
P[:,:] = dotn(A,P,tp(A)) + M.Q;
# Update
S[:,:] = dotn(C,P,tp(C)) + M.R;
K[:,:] = dotn(P,tp(C),inv(S));
xu[:,k] = xp[:,k] + dotn(K,y[:,k] - yp[:,k]);
yu[:,k] = M.h(xu[:,k ],u[:,k ],th);
P[:,:] = dotn(eye(M.nx)-dotn(K,C),P);
return K,P,S,xp,xu,yp,yu;
def __init__(self,name="Nameless System",typ='ltiss',dt=1e-3,
dim=[0,0,0,0], f=[],A=[],B=[],F=[],h=[],C=[],D=[],H=[],\
th=[],Q=[],R=[],Qx=[],Qth=[]):
self.nx = dim[0]
self.nu = dim[1]
self.ny = dim[2]
self.nth = dim[3]
self.name = name
self.typ = typ
self.dt = dt
self.f = f
self.h = h
self.A = A
self.B = B
self.C = C
self.D = D
self.F = F
self.H = H
if isempty(th):
self.th = zeros(self.nth)
else:
self.th = th
self.setQ(Q)
self.setR(R)
return
def __generateSequence(self,T,seed=[]):
# Set noise reproducibility seed
if not isempty(seed):
random.seed(seed);
# Initialize sizes and data storage
n = len(self._pd[:,0]);
N = int(T/self._dt+1);
wout = zeros((n,N));
minsamps = int(1e6);
# Generate random sequences (with white frequency)
if self._d == 1:
w = random.normal(0,1,n*max([N,minsamps]));
elif self._d == 2:
w = random.uniform(-0.5,0.5,n*max([N,minsamps]));
w = reshape(w,(n,max([N,minsamps])));
# Include frequency characcteristics into sequences
for i in range(0,n):
if self._f == 1: # ... with 1 frequency
wout[i,:] = w[i,0:N]; # sequence is already correct;
elif self._f >= 2 and self._f <= 6:
t_temp = linspace(0,max([N,minsamps])*self._dt, max([N,minsamps]));
f = fft.rfftfreq(t_temp.shape[-1],self._dt)*2;
A = fft.rfft(w[i,:]);
for j in range(0,len(f)):
if f[j] != 0: A[j] = A[j]*(f[j]**(self._pf/20));
w[i,:] = fft.irfft(A);
# f,A = self.__myFFT(w[i,:],t_temp,1);
# w[i,:] = fft.irfft(A);
# Shorten sequence
wout[i,:] = w[i,0:N];
# Include auto-correlation characteristics into sequences
if self._a == 1: # No autocorrellation
pass;
elif self._a == 2: # Causal integration
for i in range(1,N):
wout[:,i] = wout[:,i-1] + self._dt*wout[:,i]
elif self._a == 3: # Gaussian auto-correlation
wout = self.__correlateGaussian(wout);
elif self._a == 4: # block auto-correlation
wout = self.__correlateBlock(wout);
# Rescale
for i in range(0,n):
wout[i,:] = wout[i,:]-mean(wout[i,:]);
if self._d == 1:
wout[i,:] = wout[i,:]/std(wout[i,:]);
elif self._d == 2:
wout[i,:] = wout[i,:]/(max(wout[i,:])-min(wout[i,:]));
wout[i,:] = self._pd[i,1]*wout[i,:] + self._pd[i,0];
self._w = wout;
return 0;
def myplot(ax,x,y,c=tudcols[0],xlab='',ylab='',tit='',scal='linlin',xtx = [],ytx = []):
plt.sca(ax);
plt.plot(x,y,color=c);
ax.xaxis.set_label_coords(0.5, -0.05);
if scal == 'linlog' or scal == 'loglog':
ax.set_yscale('log');
if scal == 'loglin' or scal == 'loglog':
ax.set_xscale('log');
ax.tick_params(which='both',direction='in');
if not isempty(xtx):
ax.set_xticks(xtx[0]);
ax.set_xticklabels(xtx[1]);
if not isempty(ytx):
ax.set_yticks(ytx[0]);
ax.set_yticklabels(ytx[1]);
ax.set_title(tit,pad=-16);
ax.set_ylabel(ylab);
ax.set_xlabel(xlab);
ax.set_xlim(x[[0,-1]]);
h = max([max(y)-min(y),0.1]);
ax.set_ylim([min(y)-0.1*h,min(y) + 1.3*h]);
def generalize(M,p,S=[]):
# Fetch everything from model
f = M.f;
A = M.A;
B = M.B;
F = M.F;
h = M.h;
C = M.C;
D = M.D;
H = M.H;
th = M.th;
nx = M.nx
nu = M.nu;
ny = M.ny;
nth = M.nth;
name = M.name;
typ = M.typ;
dt = M.dt;
Q = M.Q;
R = M.R;
# Generalize all system functions and matrices
# State transition function
def f_t(x,u,th):
x_t = zeros(p*nx);
for i in range(0,p):
x_t[i*nx:(i+1)*nx] = f(x[i*nx:(i+1)*nx],u[i*nu:(i+1)*nu],th);
return x_t;
# State transition function to state gradient
def A_t(x,u,th):
A_t = zeros((p*nx,p*nx));
for i in range(0,p):
A_t[i*nx:(i+1)*nx,i*nx:(i+1)*nx] = \
A(x[i*nx:(i+1)*nx],u[i*nu:(i+1)*nu],th);
return A_t;
# State transition function to input gradient
def B_t(x,u,th):
B_t = zeros((p*nx,p*nu));
for i in range(0,p):
B_t[i*nx:(i+1)*nx,i*nu:(i+1)*nu] = \
B(x[i*nx:(i+1)*nx],u[i*nu:(i+1)*nu],th);
return B_t;
# State transition function to parameter gradient
def F_t(x,u,th):
F_t = zeros((p*nx,nth));
for i in range(0,p):
F_t[i*nx:(i+1)*nx,:] = \
F(x[i*nx:(i+1)*nx],u[i*nu:(i+1)*nu],th);
return F_t;
# Output function
def h_t(x,u,th):
y_t = zeros(p*ny);
for i in range(0,p):
y_t[i*nx:(i+1)*nx] = h(x[i*nx:(i+1)*nx],u[i*nu:(i+1)*nu],th);
return y_t;
# Output function to state gradient
def C_t(x,u,th):
C_t = zeros((p*ny,p*nx));
for i in range(0,p):
C_t[i*ny:(i+1)*ny,i*nx:(i+1)*nx] = \
C(x[i*nx:(i+1)*nx],u[i*nu:(i+1)*nu],th);
return C_t;
# Output function to input gradient
def D_t(x,u,th):
D_t = zeros((p*ny,p*nu));
for i in range(0,p):
D_t[i*ny:(i+1)*ny,i*nu:(i+1)*nu] = \
D(x[i*nx:(i+1)*nx],u[i*nu:(i+1)*nu],th);
return D_t;
# Measurement function to parameter gradient
def H_t(x,u,th):
H_t = zeros((p*ny,nth));
for i in range(0,p):
H_t[i*ny:(i+1)*ny,:] = \
H(x[i*nx:(i+1)*nx],u[i*nu:(i+1)*nu],th);
return H_t;
# Include corellation information if specified
if not isempty(Q) and not isempty(R):
if not isempty(S):
Pw = kron(S,inv(Q));
Pz = kron(S,inv(R));
else:
Pw = kron(eye(p),inv(Q));
Pz = kron(eye(p),inv(R));
else:
Pw = [];
Pz = [];
M_t = Model('Generalized '+ name, typ, dt, [p*nx,p*nu,p*ny,nth], \
f_t,A_t,B_t,F_t,h_t,C_t,D_t,H_t,th,Pw,Pz);
return M_t, kron(eye(p,p,1),eye(int(M_t.nx/p)));
def plotPE(t,xe,ye,xr,yr,the,thr,maxit,col=defcols[1]):
xbet=[0.02,0.005];
ybet=[0.05,0.005];
lef=0.03;
bot=0.06;
top = 0.05;
try:
nth = len(th[:,0]);
except:
if isempty(th):
nth = 0;
else:
nth = len(th);
th0 = zeros(nth)
th0[:] = th[:];
th = zeros((nth,maxit));
for i in range(0,nth):
th[i,:] = th0[i];
try:
nx = len(x[:,0]);
except:
nx = 0;
try:
ny = len(y[:,0]);
except:
ny = 0;
it = linspace(1,maxit,maxit);
xtx = [0.1*maxit,0.9*maxit];
xtcklabs = ['%d' % xtx[0],'%d' % xtx[1]];
rat = 3/7;
hp = rat-top-ybet[0];
wp = (1-lef-(nth-1)*xbet[0]-xbet[1])/nth
hs = (1-rat-bot-(nx+ny)*ybet[1])/(nx+ny);
ws = 1-lef-xbet[1];
for i in range(0,nth):
ax = plt.axes([lef+i*(wp+xbet[0]),1-rat+ybet[0],wp,hp])
#mySubPlot2([1,1],[nplots,1],[1,1],[nplots-i,1],xbet,ybet,left,bottom,top);
plt.plot(it,th[i,:],color=col,linestyle=styl);
if setlims:
plt.xlim([1,maxit]);
ymin = min(th[i,:]);
h = max(th[i,:]) - ymin;
if h < 1e-1:
ymin = 0;
h = max(th[i,:]);
plt.ylim([ymin - 0.1*h,ymin + 1.6*h]);
if nth == 1:
plt.title('Parameter: \u03b8', pad=-16);
else:
plt.title('Parameter ' + str(i+1) +': \u03b8$_{' + str(i+1) + '}$',pad=-16);
ax.tick_params(which='both',direction='in');
plt.xticks(xtx,xtcklabs)
ax.xaxis.set_label_coords(0.5, -0.03);
ax.set_xlabel('iter.');
for i in range(0,ny):
ax = plt.axes([lef,bot+(nx+ny-1-i)*(hs+ybet[1]),ws,hs])
#ax = mySubPlot2([1,1],[nplots,1],[1,1],[nplots-nth-i,1],xbet,ybet,left,bottom,top);
plt.plot(t,y[i,:],color=col,linestyle=styl);
if setlims:
plt.xlim([0,t[-1]]);
ymin = min(y[i,:]);
h = max(y[i,:]) - ymin;
plt.ylim([ymin - 0.1*h,ymin + 1.6*h]);
plt.xticks([],[]);
if ny == 1:
plt.title('Output: y',pad=-16);
else:
plt.title('Output '+ str(i+1) + ': $y_' + str(i+1) + '$',pad=-16);
ax.tick_params(which='both',direction='in');
for i in range(0,nx):
ax = plt.axes([lef,bot+(nx-1-i)*(hs+ybet[1]),ws,hs])
#ax = mySubPlot2([1,1],[nplots,1],[1,1],[nplots-nth-ny-i,1],xbet,ybet,left,bottom,top);
plt.plot(t,x[i,:],color=col,linestyle=styl);
if setlims:
plt.xlim([0,t[-1]]);
ymin = min(x[i,:]);
h = max(x[i,:]) - ymin;
plt.ylim([ymin - 0.1*h,ymin + 1.6*h]);
if i != nx-1:
plt.xticks([],[]);
else:
plt.xlabel('t (s)')
ax.xaxis.set_label_coords(0.5, -0.05);
if nx == 1:
plt.title('Hidden state: x',pad=-16);
else:
plt.title('Hidden state '+ str(i+1) + ': $x_' + str(i+1) + '$',pad=-16);
ax.tick_params(which='both',direction='in');
plt.xlabel("t (s)");
def plotOPE(t,x,y,th,col=defcols[1],setlims=True,styl='-'):
xbet=[0.01,0];
ybet=[0.005,0.005];
left=0.03;
bottom=0.06;
top = 0.05;
N = len(t);
try:
nth = len(th[:,0]);
except:
if isempty(th):
nth = 0;
else:
nth = len(th);
th0 = zeros(nth)
th0[:] = th[:];
th = zeros((nth,N));
for i in range(0,nth):
th[i,:] = th0[i];
try:
nx = len(x[:,0]);
except:
nx = 0;
try:
ny = len(y[:,0]);
except:
ny = 0;
nplots = nth + nx + ny;
for i in range(0,nth):
ax = mySubPlot2([1,1],[nplots,1],[1,1],[nplots-i,1],xbet,ybet,left,bottom,top);
plt.plot(t,th[i,:],color=col,linestyle=styl);
plt.xticks([],[]);
if setlims:
plt.xlim([0,t[-1]]);
ymin = min(th[i,:]);
h = max(th[i,:]) - ymin;
if h < 1e-1:
ymin = 0;
h = max(th[i,:]);
plt.ylim([ymin - 0.1*h,ymin + 1.6*h]);
if nth == 1:
plt.title('Parameter: \u03b8', pad=-16);
else:
plt.title('Parameter ' + str(i+1) +': \u03b8$_{' + str(i+1) + '}$',pad=-16);
ax.tick_params(which='both',direction='in');
for i in range(0,ny):
ax = mySubPlot2([1,1],[nplots,1],[1,1],[nplots-nth-i,1],xbet,ybet,left,bottom,top);
plt.plot(t,y[i,:],color=col,linestyle=styl);
if setlims:
plt.xlim([0,t[-1]]);
ymin = min(y[i,:]);
h = max(y[i,:]) - ymin;
plt.ylim([ymin - 0.1*h,ymin + 1.6*h]);
plt.xticks([],[]);
if ny == 1:
plt.title('Output: y',pad=-16);
else:
plt.title('Output '+ str(i+1) + ': $y_' + str(i+1) + '$',pad=-16);
ax.tick_params(which='both',direction='in');
for i in range(0,nx):
ax = mySubPlot2([1,1],[nplots,1],[1,1],[nplots-nth-ny-i,1],xbet,ybet,left,bottom,top);
plt.plot(t,x[i,:],color=col,linestyle=styl);
if setlims:
plt.xlim([0,t[-1]]);
ymin = min(x[i,:]);
h = max(x[i,:]) - ymin;
plt.ylim([ymin - 0.1*h,ymin + 1.6*h]);
if i != nx-1:
plt.xticks([],[]);
else:
plt.xlabel('t (s)')
ax.xaxis.set_label_coords(0.5, -0.05);
if nx == 1:
plt.title('Hidden state: x',pad=-16);
else:
plt.title('Hidden state '+ str(i+1) + ': $x_' + str(i+1) + '$',pad=-16);
ax.tick_params(which='both',direction='in');
plt.xlabel("t (s)");