def ukf_test(dt,tf,mux0,P0,YK,Qk,Rk):
global nameBit
# add in this functionality so we can change the propagation function dependent on the nameBit ... may or may not be needed
if nameBit == 1:
# create UKF object
UKF = ukf.ukf(2,0,1,eqom_ukf,Qk)
elif nameBit == 2:
# create UKF object
UKF = ukf.ukf(2,0,1,eqom_ukf,Qk)
elif nameBit == 3:
# create UKF object
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)
xf = np.zeros((nSteps,2))
Pf = np.zeros((nSteps,4))
tk = np.arange(0.0,tf,dt)
xf[0,:] = UKF.xhat.copy()
Pf[0,:] = UKF.Pk.reshape((4,))
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
UKF.sync(dt,ym,measurement_ukf,Rk,True)
# copy
#if k < nSteps-1:
xf[k,:] = UKF.xhat.copy()
Pf[k,:] = UKF.Pk.reshape((4,))
t2 = time.time()
print("Elapsed time: %f sec" % (t2-t1))
return(xf,Pf)
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 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 main(argin='./'):
# output file
FOUT = open('python_benchmark.csv','w')
FOUT.write('t,x1,x2,ymeas1,ymeas2,x1hat,x2hat,P11,P22\n');
# initialize UKF
#Qkin = np.array([[0.2]])
Qkin = np.array([[20.0]])
UKF = ukf.ukf(2,1,1,stateDerivativeEKF,Qk = Qkin)
Rkin = np.array([ [sigma_y1*sigma_y1] ])
dt = 0.01
tfin = 10.0
nSteps = int(tfin/dt)
tsim = 0.0
xk = np.array([1.0,0.0])
yk = measFunction(xk,tsim,np.array([0.0]))
UKF.init(yk,initFunction,tsim)
print(nSteps)
xkl = np.zeros((nSteps,2))
xtl = np.zeros((nSteps,2))
Pkl = np.zeros((nSteps,2))
tl = np.zeros((nSteps,))
for k in range(nSteps):
# simulate
y = sp.odeint(stateDerivative,xk,np.array([tsim,tsim+dt]),args=([],) )
xk = y[-1,:].copy()
# update time
tsim = tsim + dt
# measurement
ymeas = simMeasurementFunction(xk,tsim)
# sync UKF to current time
UKF.sync(dt,ymeas,measFunction,Rkin)
# log to file
FOUT.write('%f,%f,%f,%f,%f,%f,%f,%f\n' % (tsim,xk[0],xk[1],ymeas[0],UKF.xhat[0],UKF.xhat[1],UKF.Pk[0,0],UKF.Pk[1,1]) )
# log to data
xkl[k,0] = UKF.xhat[0]
xkl[k,1] = UKF.xhat[1]
xtl[k,0] = xk[0]
xtl[k,1] = xk[1]
Pkl[k,0] = UKF.Pk[0,0]
Pkl[k,1] = UKF.Pk[1,1]
tl[k] = tsim
print("Completed sim")
fig = plt.figure()
ax = []
for k in range(2):
nam = 'e'+str(k+1)
ax.append(fig.add_subplot(2,1,k+1,ylabel=nam))
ax[k].plot(tl,xkl[:,k]-xtl[:,k])
ax[k].plot(tl,3*np.sqrt(Pkl[:,k]),'r--')
ax[k].plot(tl,-3*np.sqrt(Pkl[:,k]),'r--')
ax[k].grid()
fig.show()
raw_input("Return to continue")
FOUT.close()
return
pos_ref = np.zeros(3)
ref = np.array([0.0, 0.0, 3.0, 0.0]) # x,y,z,yaw
# Initialize UKF
##########################################################
x0 = np.array([
qrb.pos[0], qrb.pos[1], qrb.pos[2], qrb.rpy[0], qrb.rpy[1],
qrb.rpy[2] + math.pi * 0.8
])
P0 = np.diag([100.0, 100.0, 100.0, 0.01, 0.01, 9.0])
Q = np.diag([0.01, 0.01, 0.01, 0.001, 0.001, 0.001])
R = np.diag([0.002, 0.002, 0.005])
alpha = 1 * 10**(-3)
kappa = 0
beta = 2.0
filter = ukf.ukf(x0, P0, Q, R, alpha, kappa, beta)
# Predefined controller references - for testing
##########################################################
arg_parser = argparse.ArgumentParser()
arg_parser.add_argument("ref_mode",
help=""" Choose between an angle reference
template. Values are: step, ramp, sin, manual """)
args = arg_parser.parse_args()
if args.ref_mode == "step":
name2 = "step"
step_1 = StepAndRampMetaSignal(3, 13, 20)
step_2 = StepAndRampMetaSignal(63, 73, -20)
