def ekf_test(dt,tf,mux0,P0,YK,Qk,Rk):
#Qk = np.array([[1.0*dt]])
#Rk = np.array([[1.0]])
# create EKF object
EKF = ekf.ekf(2,0,eqom_ekf,eqom_jacobian_ekf,eqom_gk_ekf,Qk)
nSteps = int(tf/dt)+1
ts = 0.0
# initialize EKF
EKF.init_P(mux0,P0,ts)
# initialize performance object
simOut = trials_processing.simOutput()
xf = np.zeros((nSteps,2))
Pf = np.zeros((nSteps,2,2))
tk = np.arange(0.0,tf,dt)
xf[0,:] = EKF.xhat.copy()
Pf[0,:,:] = EKF.Pk.copy()
t1 = time.time()
for k in range(1,nSteps):
# get the new measurement
ym = np.array([YK[k]])
ts = ts + dt
# sync the EKF, with continuous-time integration
EKF.propagateOde(dt)
#EKF.propagateRK4(dt)
EKF.update(ts,ym,measurement_ekf,measurement_gradient,Rk)
# check that the eigenvalukes are reasonably bounded
w = np.linalg.eigvalsh(EKF.Pk.copy())
for jj in range(len(w)):
if math.fabs(w[jj]) > 1.0e6:
simOut.fail_singular_covariance(k)
print("Covariance eigenvalue too large, t = %f" % (ts))
return(xf,Pf,simOut)
# copy
xf[k,:] = EKF.xhat.copy()
Pf[k,:,:] = EKF.Pk.copy()
t2 = time.time()
print("Elapsed time: %f sec" % (t2-t1))
simOut.complete(nSteps)
return(xf,Pf,simOut)
def ukf_test(dt,tf,mux0,P0,YK,Qk,Rk):
UKF = ukf.ukf(2,0,1,eqom_ukf,Qk)
nSteps = int(tf/dt)+1
ts = 0.0
# initialize UKF
UKF.init_P(mux0,P0,ts)
# initialize performance object
simOut = trials_processing.simOutput()
xf = np.zeros((nSteps,2))
Pf = np.zeros((nSteps,2,2))
tk = np.arange(0.0,tf,dt)
xf[0,:] = UKF.xhat.copy()
Pf[0,:,:] = UKF.Pk.copy()
t1 = time.time()
for k in range(1,nSteps):
# get the new measurement
ym = np.array([YK[k]])
ts = ts + dt
# sync the UKF, with continuous-time integration
try:
UKF.sync(dt,ym,measurement_ukf,Rk,True)
except np.linalg.linalg.LinAlgError:
print("Singular covariance at t = %f" % (ts))
simOut.fail_singular_covariance(k)
return(xf,Pf,simOut)
# check that the eigenvalukes are reasonably bounded
w = np.linalg.eigvalsh(UKF.Pk.copy())
for jj in range(len(w)):
if math.fabs(w[jj]) > 1.0e6:
simOut.fail_singular_covariance(k)
print("Covariance eigenvalue too large, t = %f" % (ts))
return(xf,Pf,simOut)
# copy
#if k < nSteps-1:
xf[k,:] = UKF.xhat.copy()
Pf[k,:,:] = UKF.Pk.copy()
t2 = time.time()
print("Elapsed time: %f sec" % (t2-t1))
simOut.complete(nSteps)
return(xf,Pf,simOut)
def enkf_test(dt,tf,mux0,P0,YK,Qk,Rk,flag_adapt=False):
global nameBit
# measurement influence matrix
Hk = np.array([ [1.0,0.0] ])
eqom_use = eqom_enkf
if flag_adapt:
ENKF = enkf.adaptive_enkf(2,0,eqom_use,Hk,Qk,Rk,Ns=100)
else:
# create nonadaptive EnKF object
ENKF = enkf.enkf(2,0,eqom_use,Hk,Qk,Rk,Ns=100)
nSteps = int(tf/dt)+1
ts = 0.0
#initialize EnKF
ENKF.init(mux0,P0,ts)
# initialize performance object
simOut = trials_processing.simOutput()
xf = np.zeros((nSteps,2))
Pf = np.zeros((nSteps,2,2))
Nf = np.zeros(nSteps)
XK = np.zeros((nSteps,2,ENKF._N))
tk = np.arange(0.0,tf,dt)
#get the mean and covariance estimates
Nf[0] = ENKF.get_N()
xf[0,:] = np.mean(ENKF.xk,axis=1)
Pxx = np.zeros((2,2))
for k in range(ENKF.get_N()):
Pxx = Pxx + 1.0/(1.0+float(ENKF._N))*np.outer(ENKF.xk[:,k]-xf[0,:],ENKF.xk[:,k]-xf[0,:])
Pf[0,:,:] = Pxx.copy()
t1 = time.time()
for k in range(1,nSteps):
# get the new measurement
ym = np.array([YK[k]])
ts = ts + dt
# sync the ENKF, with continuous-time integration
# propagate filter
ENKF.propagateOde(dt)
#ENKF.propagate(dt)
# update
ENKF.update(ym)
# log
xf[k,:] = np.mean(ENKF.xk,axis=1)
Pxx = np.zeros((2,2))
for kj in range(ENKF.get_N()):
Pxx = Pxx + 1.0/(float(ENKF._N)-1.0)*np.outer(ENKF.xk[:,kj]-xf[k,:],ENKF.xk[:,kj]-xf[k,:])
Pf[k,:,:] = Pxx.copy()
Nf[k] = ENKF.get_N()
# check that the eigenvalukes are reasonably bounded
w = np.linalg.eigvalsh(Pf[k,:,:].copy())
for jj in range(len(w)):
if math.fabs(w[jj]) > 1.0e6:
simOut.fail_singular_covariance(k)
print("Covariance eigenvalue too large, t = %f" % (ts))
return(xf,Pf,Nf,XK,simOut)
if not flag_adapt:
XK[k,:,:] = ENKF.xk.copy()
t2 = time.time()
print("Elapsed time: %f sec" % (t2-t1))
simOut.complete(nSteps)
return(xf,Pf,Nf,XK,simOut)
def sir_test(dt,tf,mux0,P0,YK,Qk,Rk,Nparticles = 100):
global mux_sample
global P_sample
global Ru
global Qu
Ru = Rk.copy()
Qu = Qk.copy()
mux_sample = mux0.copy()
P_sample = P0.copy()
# number of particles
Nsu = Nparticles
# add in this functionality so we can change the propagation function dependent on the nameBit ... may or may not be needed
# create SIR object
SIR = sir.sir(2,Nsu,eqom_use,processNoise,measurementPdf)
nSteps = int(tf/dt)+1
ts = 0.0
# initialize the particle filter
SIR.init(initialParticle)
# initialize performance object
simOut = trials_processing.simOutput()
# the estimate (weighted mean)
#xf = np.zeros((nSteps,2))
#tk = np.arange(0.0,tf,dt)
px1 = np.zeros((nSteps,SIR.Ns))
px2 = np.zeros((nSteps,SIR.Ns))
weights = np.zeros((nSteps,SIR.Ns))
px1[0,:] = SIR.XK[0,:].copy()
px2[0,:] = SIR.XK[1,:].copy()
weights[0,:] = SIR.WI.copy()
t1 = time.time()
for k in range(1,nSteps):
# get the new measurement
ym = np.array([YK[k]])
ts = ts + dt
# call SIR
SIR.update(dt,ym)
# store
px1[k,:] = SIR.XK[0,:].copy()
px2[k,:] = SIR.XK[1,:].copy()
weights[k,:] = SIR.WI.copy()
# resample
SIR.sample()
t2 = time.time()
print("Elapsed time: %f sec" % (t2-t1))
simOut.complete(nSteps)
# sort out the most likely particle at each time
xml = np.zeros((nSteps,2))
for k in range(nSteps):
idxk = np.argmax(weights[k,:])
xml[k,0] = px1[k,idxk]
xml[k,1] = px2[k,idxk]
# compute the mean and covariance over time
mux = np.zeros((nSteps,2))
Pxx = np.zeros((nSteps,2,2))
for k in range(nSteps):
mux[k,0] = np.sum( np.multiply(px1[k,:],weights[k,:]) )
mux[k,1] = np.sum( np.multiply(px2[k,:],weights[k,:]) )
Pxk = np.zeros((2,2))
for j in range(Nsu):
iv = np.array([ px1[k,j]-mux[k,0],px2[k,j]-mux[k,1] ])
Pxk = Pxk + weights[k,j]*np.outer(iv,iv)
Pxx[k,:,:] = Pxk.copy()
return(mux,Pxx,px1,px2,weights,simOut)
