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)