def execute_sim(function,Ts,tf,Nsims,Pi,name=None,cluster=False,informative=True):
nSteps = int(tf/Ts)+1
## matrix of initial conditions, size 2 x N
X0 = np.random.multivariate_normal(mux0,Pi,size=(Nsims,)).transpose()
## simulation output/measurement times
tsim = np.arange(0.0,tf+Ts,Ts)
## simulation output measurements
YK = np.zeros((nSteps,Nsims))
## simulation state history
XK = np.zeros((nSteps,2*Nsims))
t1 = time.time()
for k in range(Nsims):
if not cluster:
sim = cp_dynamics.cp_simObject(function,X0[:,k],Ts)
if cluster:
if informative:
sim = cp_dynamics.cp_simObjectCluster(function,X0[:,k],Ts)
else:
sim = cp_dynamics.cp_simObjectNonInformative(function,X0[:,k],Ts)
# simulate
(YK[:,k],XK[:,(2*k):(2*k+2)],tk) = sim.simFull(Tf=tf)
t2 = time.time()
etaCalc(k,Nsims,t2-t1)
t2 = time.time()
print("Completed %d sims in %g secs" % (Nsims,t2-t1))
return(tsim,XK,YK,mux0,Ts,tf)
def ukf_test(dt = 0.01):
Qk = np.array([[1.0e-2]])
Rk = np.array([[1.0]])
# create UKF object
UKF = ukf.ukf(2,0,1,eqom_ukf,Qk)
P0 = np.array([ [0.1, 1.0e-6],[1.0e-6, 1.0] ])
mux0 = np.array([0.0,0.0])
x0 = np.random.multivariate_normal(mux0,P0)
sim = cp_dynamics.cp_simObject(cp_dynamics.eqom_stoch,x0,dt)
tf = 30.0
nSteps = int(tf/dt)
ts = 0.0
# initialize UKF
UKF.init_P(mux0,P0,ts)
xk = np.zeros((nSteps,2))
xf = np.zeros((nSteps,2))
Pf = np.zeros((nSteps,4))
tk = np.arange(0.0,tf,dt)
xk[0,:] = x0.copy()
xf[0,:] = UKF.xhat.copy()
Pf[0,:] = UKF.Pk.reshape((4,))
t1 = time.time()
for k in range(nSteps):
# step the simulation and take a measurement
(ym,x) = sim.step()
ts = ts + dt
# sync the UKF, with continuous-time integration
UKF.sync(dt,ym,measurement_ukf,Rk,True)
# copy
if k < nSteps-1:
xf[k+1,:] = UKF.xhat.copy()
Pf[k+1,:] = UKF.Pk.reshape((4,))
xk[k+1,:] = x.copy()
t2 = time.time()
print("Elapsed time: %f sec" % (t2-t1))
fig1 = plt.figure()
print(tk.shape)
print(xk.shape)
ax = []
for k in range(4):
if k < 2:
nam = 'x' + str(k+1)
else:
nam = 'e' + str(k-1)
ax.append(fig1.add_subplot(2,2,k+1,ylabel=nam))
if k < 2:
ax[k].plot(tk,xk[:,k])
ax[k].plot(tk,xf[:,k],'m--')
else:
ax[k].plot(tk,xk[:,k-2]-xf[:,k-2])
ax[k].plot(tk,3.0*np.sqrt(Pf[:,3*(k-2)]),'r--')
ax[k].plot(tk,-3.0*np.sqrt(Pf[:,3*(k-2)]),'r--')
ax[k].grid()
fig1.show()
# compute the unit variance transformation of the error
e1 = np.zeros((nSteps,2))
chi2 = np.zeros(nSteps)
for k in range(nSteps):
P = Pf[k,:].reshape((2,2))
R = np.linalg.cholesky(P)
e1[k,:] = np.dot(R,(xk[k,:]-xf[k,:]))
chi2[k] = np.dot(e1[k,:],e1[k,:])
(W,p) = stats.shapiro(e1.reshape((2*nSteps,)))
print("Shapiro-Wilk output for all residuals: W = %f, p = %g" % (W,p) )
for k in range(2):
(W,p) = stats.shapiro(e1[:,k])
print("Shapiro-Wilk output for axis %d: W = %f, p = %g" % (k,W,p) )
fig2 = plt.figure()
ax = []
for k in range(2):
nam = 'et' + str(k+1)
ax.append(fig2.add_subplot(1,2,k+1,ylabel = nam))
ax[k].plot(tk,e1[:,k])
ax[k].grid()
fig2.show()
raw_input("Return to quit")
print("Leaving ukf_test")
return
def generate_sim(function,Ts,tf,name=None,cluster=False,informative=True):
nSteps = int(tf/Ts)+1
## matrix of initial conditions, size 2 x N
if not cluster:
X0 = np.random.multivariate_normal(mux0,P0,size=(Ns,)).transpose()
else:
X0 = np.random.multivariate_normal(mux0,Pcluster,size=(Ns,)).transpose()
## simulation output/measurement times
tsim = np.arange(0.0,tf+Ts,Ts)
## simulation output measurements
YK = np.zeros((nSteps,Ns))
## simulation state history
XK = np.zeros((nSteps,2*Ns))
t1 = time.time()
for k in range(Ns):
if not cluster:
sim = cp_dynamics.cp_simObject(function,X0[:,k],Ts)
if cluster:
if informative:
sim = cp_dynamics.cp_simObjectCluster(function,X0[:,k],Ts)
else:
sim = cp_dynamics.cp_simObjectNonInformative(function,X0[:,k],Ts)
# simulate
(YK[:,k],XK[:,(2*k):(2*k+2)],tk) = sim.simFull(Tf=tf)
t2 = time.time()
etaCalc(k,Ns,t2-t1)
t2 = time.time()
print("Completed %d sims in %g secs" % (Ns,t2-t1))
if name is not None:
# write to file
# write settings
FID = open(name + "_settings.ini",'w')
FID.write("[%s]\n" % (name))
FID.write("Function: " + function.__name__ + "\n")
FID.write("Ts: %g\n" % (Ts))
FID.write("tf: %g\n" % tf)
FID.write("Ns: %d\n" % Ns)
if not cluster:
FID.write("P0_11: %f\n" % P0[0,0])
FID.write("P0_12: %f\n" % P0[0,1])
FID.write("P0_21: %f\n" % P0[1,0])
FID.write("P0_22: %f\n" % P0[1,1])
else:
FID.write("P0_11: %f\n" % Pcluster[0,0])
FID.write("P0_12: %f\n" % Pcluster[0,1])
FID.write("P0_21: %f\n" % Pcluster[1,0])
FID.write("P0_22: %f\n" % Pcluster[1,1])
FID.write("mux_1: %f\n" % mux0[0])
FID.write("mux_2: %f\n" % mux0[1])
FID.close()
print("Wrote settings file")
# write data
datafilename = name + "_data.csv"
FID = open(datafilename,'w')
for k in range(nSteps):
FID.write("%f," % tsim[k])
for j in range(Ns):
FID.write("%f,%f,%f," % (XK[k,2*j],XK[k,2*j+1],YK[k,j]))
FID.write("\n")
FID.close()
print("Wrote data file")
pass
else:
# plot histories and measurements
fig = plt.figure()
ax = []
for k in range(2):
nam = 'x' + str(k+1)
ax.append(fig.add_subplot(1,2,k+1,ylabel=nam))
inplot1 = range(k,2*Ns,2)
print(inplot1)
ax[k].plot(tsim,XK[:,inplot1])
if k == 0:
ax[k].plot(tsim,YK)
fig.show()
raw_input("Return to continue")
return
