Example #1
0
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)
Example #2
0
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)
Example #3
0
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)
Example #4
0
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)