Python numpy.unwrap函数代码示例

	def correctDomain(self):
		'''
		Correct the domain of the pathphase values so that they lie
		within the [0, 2PI] domain -- [0. 6.28]
		'''
		def correctPhaseDomain(phaseValue):
			'''
			Corrects the values phase so that it resides 
			between topDomain and bottomDomain
			'''
			topDomain = 1.
			bottomDomain = -1.

			domainRange = topDomain - bottomDomain

			phaseValue %= domainRange
			if phaseValue >= topDomain or phaseValue <= bottomDomain:
				if phaseValue > 0:
						phaseValue -= domainRange
				elif phaseValue <= 0:
				    phaseValue += domainRange
			return phaseValue

		topDomain = self.topDomain

		self.consistentDomainValues = self.values[:]
		self.consistentDomainValues2 = self.values[:]
		#use modulo 2PI to maintain a consistent domain
		self.consistentDomainValues = [ (i + (2 *numpy.pi)) % topDomain for i in self.consistentDomainValues]


		self.consistentDomainValues2 = [ correctPhaseDomain(i) for i in self.consistentDomainValues2]

		self.correctedValues=numpy.unwrap(self.consistentDomainValues)
		self.correctedValues2=numpy.unwrap(self.consistentDomainValues2)

 def plotall(self):
     real = self.z_data_raw.real
     imag = self.z_data_raw.imag
     real2 = self.z_data_sim.real
     imag2 = self.z_data_sim.imag
     fig = plt.figure(figsize=(15,5))
     fig.canvas.set_window_title("Resonator fit")
     plt.subplot(131)
     plt.plot(real,imag,label='rawdata')
     plt.plot(real2,imag2,label='fit')
     plt.xlabel('Re(S21)')
     plt.ylabel('Im(S21)')
     plt.legend()
     plt.subplot(132)
     plt.plot(self.f_data*1e-9,np.absolute(self.z_data_raw),label='rawdata')
     plt.plot(self.f_data*1e-9,np.absolute(self.z_data_sim),label='fit')
     plt.xlabel('f (GHz)')
     plt.ylabel('Amplitude')
     plt.legend()
     plt.subplot(133)
     plt.plot(self.f_data*1e-9,np.unwrap(np.angle(self.z_data_raw)),label='rawdata')
     plt.plot(self.f_data*1e-9,np.unwrap(np.angle(self.z_data_sim)),label='fit')
     plt.xlabel('f (GHz)')
     plt.ylabel('Phase')
     plt.legend()
     # plt.gcf().set_size_inches(15,5)
     plt.tight_layout()
     plt.show()

    def plot(self):

        # Plot the estimated and ground truth trajectories
        ground_truth = plt.plot(self.XYT[:, 0], self.XYT[:, 1], 'g.-', label='Ground Truth')
        mean_trajectory = plt.plot(self.MU[:, 0], self.MU[:, 1], 'r.-', label='Estimate')
        plt.legend()

        # Try changing this to different standard deviations
        sigma = 1 # 2 or 3

        # Plot the errors with error bars
        Error = self.XYT-self.MU
        T = range(size(self.XYT,0))
        f, axarr = plt.subplots(3, sharex=True)
        axarr[0].plot(T,Error[:,0],'r-')
        axarr[0].plot(T,sigma*sqrt(self.VAR[:,0]),'b--')
        axarr[0].plot(T,-sigma*sqrt(self.VAR[:,0]),'b--')
        axarr[0].set_title('X error')
        axarr[0].set_ylabel('Error (m)')
        
        axarr[1].plot(T,Error[:,1],'r-')
        axarr[1].plot(T,sigma*sqrt(self.VAR[:,1]),'b--')
        axarr[1].plot(T,-sigma*sqrt(self.VAR[:,1]),'b--')
        axarr[1].set_title('Y error')
        axarr[1].set_ylabel('Error (m)')
        
        axarr[2].plot(T,degrees(unwrap(Error[:,2])),'r-')
        axarr[2].plot(T,sigma*degrees(unwrap(sqrt(self.VAR[:,2]))),'b--')
        axarr[2].plot(T,-sigma*degrees(unwrap(sqrt(self.VAR[:,2]))),'b--')
        axarr[2].set_title('Theta error (degrees)')
        axarr[2].set_ylabel('Error (degrees)')
        axarr[2].set_xlabel('Time')
        
        plt.show()

def align_wfarr_average_phase(this,that,mask=None,verbose=False):
    '''
    'this' phase will be aligned to 'that' phase over their domains
    '''

    #
    from numpy import angle,unwrap,mean

    #
    if mask is None:
        u = this[:,1]+1j*this[:,2]
        v = that[:,1]+1j*that[:,2]
    else:
        u = this[mask,1]+1j*this[mask,2]
        v = that[mask,1]+1j*that[mask,2]

    #
    _a = unwrap( angle(u) )
    _b = unwrap( angle(v) )


    #
    a,b = mean( _a ), mean( _b )
    dphi = -a + b

    #
    if verbose:
        alert('The phase shift applied is %s radians.'%magenta('%1.4e'%(dphi)))

    #
    this_ = shift_wfarr_phase(this,dphi)

    #
    return this_

def drawDataCallback(baseline):
    matplotlib.pyplot.clf()
    acc_n,interleave_a,interleave_b = get_data(baseline)

    matplotlib.pyplot.subplot(211)
    if ifch == True:
        matplotlib.pyplot.semilogy(numpy.abs(interleave_a))
        matplotlib.pyplot.xlim(0,1024)
    else:
        matplotlib.pyplot.semilogy(xaxis,numpy.abs(interleave_a))
    matplotlib.pyplot.grid()
    matplotlib.pyplot.title('Integration number %i \n%s'%(acc_n,baseline))
    matplotlib.pyplot.ylabel('Power (arbitrary units)')

    matplotlib.pyplot.subplot(212)
    if ifch == True:
        matplotlib.pyplot.plot(numpy.unwrap(numpy.angle(interleave_b)))
        matplotlib.pyplot.xlim(0,1024)
        matplotlib.pyplot.xlabel('FFT Channel')
    else:
        matplotlib.pyplot.plot(xaxis,numpy.unwrap(numpy.angle(interleave_b)))
        matplotlib.pyplot.xlabel('FFT Frequency')
    matplotlib.pyplot.ylabel('Phase')
    matplotlib.pyplot.ylim(-180,180)
    matplotlib.pyplot.grid()



    fig.canvas.manager.window.after(100, drawDataCallback,baseline)

def chickling_pn(shotno, date=time.strftime("%Y%m%d"), bandwidth=1000):
	    
	fname, data = file_finder(shotno,date)
	
	fs = data[0]['samplerate']	
	samplesize = int(np.unwrap(data[0]['phasediff_co2']).size/bandwidth)
	phase_noise_sc = np.zeros(samplesize)
	phase_noise_ref = np.zeros(samplesize)

	#reshape the array of x points (20M for 1s) into a 2d array each with 40k segments.
	phasediff_pn_sc = np.reshape(np.unwrap(data[0]['phasediff_co2'][0:(samplesize*bandwidth)]),(samplesize,bandwidth))
	phasediff_pn_ref = np.reshape(np.unwrap(data[0]['phasediff_hene'][0:(samplesize*bandwidth)]),(samplesize,bandwidth))

	#for each horizontal column perform a standard deviation
	for i in range(0,samplesize):
		phase_noise_sc[i] = np.std(phasediff_pn_sc[i])
		phase_noise_ref[i] = np.std(phasediff_pn_ref[i])

	#plot STD against time and find the average
	plt.figure("Phase Noise for Scene shot " + str(shotno) + " with bandwidth = " + str(bandwidth) +   " Date " + str(date))
	plt.xlabel("Time, s")
	plt.ylabel("Phase Noise, mRadians")
	plt.plot(np.linspace(0,1,samplesize),phase_noise_sc*1000)
	print("Scene Phase STD = "+str(np.std(phase_noise_sc)))
	print("Scene Phase AVR = "+str(np.mean(phase_noise_sc)))

	plt.figure("Phase Noise for Ref shot " + str(shotno) + " with bandwidth = " + str(bandwidth) +   " Date " + str(date))
	plt.xlabel("Time, s")
	plt.ylabel("Phase Noise, mRadians")
	plt.plot(np.linspace(0,1,samplesize),phase_noise_ref*1000)
	print("Ref Phase STD = "+str(np.std(phase_noise_ref)))
	print("Ref Phase AVR = "+str(np.mean(phase_noise_ref)))

def chickling_corr_2ch(shotno,fileno, date=time.strftime("%Y%m%d"), bandwidth=40000):
	
	corr = np.zeros(fileno)
	for j in range (0,  fileno):
		try:
			fname, data = file_finder(shotno,date)
			samplesize = int(np.unwrap(data[0]['phasediff_co2']).size/bandwidth)
			phase_avr_co2 = np.zeros(samplesize)
			phase_avr_hene = np.zeros(samplesize)

			#reshape the array of x points (20M for 1s) into a 2d array each with 40k segments.
			phasediff_co2 = np.reshape(np.unwrap(data[0]['phasediff_co2'][0:(samplesize*bandwidth)]),(samplesize,bandwidth))
			phasediff_hene = np.reshape(np.unwrap(data[0]['phasediff_hene'][0:(samplesize*bandwidth)]),(samplesize,bandwidth))

			#for each horizontal column perform an average
			for i in range(0,samplesize):
				phase_avr_co2[i] = np.mean(phasediff_co2[i])
				phase_avr_hene[i] = np.mean(phasediff_hene[i])

			a = (phase_avr_co2 - np.mean(phase_avr_co2)) / (np.std(phase_avr_co2) * len(phase_avr_co2))
			b = (phase_avr_hene - np.mean(phase_avr_hene)) / (np.std(phase_avr_hene))
			corr[j] = np.correlate(a, b, 'valid')


			#plt.xcorr(a,b)#,'o',ms=0.4)

			#plt.figure("Correlations of Shot "+str(shotno)+" to "+ str(shotno+fileno))
			#plt.plot(np.linspace(0,1,fileno),correlation,'o')
		except Exception:
			print("~~~~~ Encountered Error, Skipping Data Set ~~~~~")
			pass
	plt.figure("Correlation for shots "+str(shotno)+" to "+str(shotno+fileno))
	plt.plot(corr,'o')

    def autophase(self, ti=None, tf=None, unwrap=False, x0=[0., 0.], adjust_f0=True):
        t = self.t
        m = self.m
        z = self.z


        if unwrap:
            phi = np.unwrap(self.phi)
        else:
            phi = self.phi

        if ti is None and tf is None:
            mask = m
        elif ti is not None and tf is None:
            mask = m & (t >= ti)
        elif ti is None and tf is not None:
            mask = m & (t < tf)
        else:
            mask = m & (t >= ti) & (t < tf)

        self.mb = mb = auto_phase(t[mask], phi[mask], x0, adjust_f0=adjust_f0)

        self.phi0 = mb[-1]

        self.phi_fit = np.polyval(mb, t)

        self.dphi = np.unwrap((
            (self.phi - self.phi_fit + np.pi) % (2*np.pi)) - np.pi)

        if adjust_f0:
            self.f0corr = self.f0 + mb[0] / (2*np.pi)
        else:
            self.f0corr = self.f0

        self._output_df_X_Y()

def chickling_corr(shotno, date=time.strftime("%Y%m%d"), bandwidth=40000):

	fname, data = file_finder(shotno,date)
		
	samplesize = int(np.unwrap(data[0]['phasediff_co2']).size/bandwidth)
	phase_avr_co2 = np.zeros(samplesize)
	phase_avr_hene = np.zeros(samplesize)

	#reshape the array of x points (20M for 1s) into a 2d array each with 40k segments.
	phasediff_co2 = np.reshape(np.unwrap(data[0]['phasediff_co2'][0:(samplesize*bandwidth)]),(samplesize,bandwidth))
	phasediff_hene = np.reshape(np.unwrap(data[0]['phasediff_hene'][0:(samplesize*bandwidth)]),(samplesize,bandwidth))

	#for each horizontal column perform an average
	for i in range(0,samplesize):
		phase_avr_co2[i] = np.mean(phasediff_co2[i])
		phase_avr_hene[i] = np.mean(phasediff_hene[i])

	x = np.linspace(0,1,samplesize)
	plt.figure("2 Channels | Blue = Scene | Orange = Reference | Green = Cross-Correlation | shot " + str(shotno) +  " Date " + str(date))
	plt.xlabel("Time, s")
	plt.ylabel("Phase Difference, Radians")
	plt.plot(x,phase_avr_co2-np.average(phase_avr_co2))
	plt.plot(x,phase_avr_hene-np.average(phase_avr_hene))

	a = (phase_avr_co2 - np.mean(phase_avr_co2)) / (np.std(phase_avr_co2) * len(phase_avr_co2))
	b = (phase_avr_hene - np.mean(phase_avr_hene)) / (np.std(phase_avr_hene))
	yc = np.correlate(a, b, 'full')
	print(np.correlate(a, b, 'valid'))
	xc = np.linspace(0,1,yc.size)
	plt.plot(xc,yc)#,'o',ms=0.4)

def calc_inst_info(modes,samplerate):
    """
Calculate the instantaneous frequency, amplitude, and phase of
each mode.
    """

    amp=np.zeros(modes.shape,np.float32)
    phase=np.zeros(modes.shape,np.float32)
    f=np.zeros(modes.shape,np.float32)

    print("Mode 1:", len(modes), samplerate)

    for m in range(len(modes)):
        h=scipy.signal.hilbert(modes[m])
        print(len(modes[m]))
        print("Mean Amplitude of mode ", m, np.mean(np.abs(h)))
        print("Mean Phase of mode ", m, np.mean(np.angle(h)))
        phase[m,:]=np.angle(h)
        print("Frequ", np.diff(np.unwrap(phase[:,np.r_[0,0:len(modes[m])]]))/(2*np.pi)*samplerate)
        amp[m,:]=np.abs(h)
        phase[m,:]=np.angle(h)
        f[m,:] = np.r_[np.nan,
                      0.5*(np.angle(-h[2:]*np.conj(h[0:-2]))+np.pi)/(2*np.pi) * samplerate,
                      np.nan]
        print("Mean Frequ of mode ", m, np.mean(np.diff(np.unwrap(phase[:,np.r_[0,0:len(modes[0])]]))/(2*np.pi)*samplerate))

        #f(m,:) = [nan 0.5*(angle(-h(t+1).*conj(h(t-1)))+pi)/(2*pi) * sr nan];

    # calc the freqs (old way)
    #f=np.diff(np.unwrap(phase[:,np.r_[0,0:len(modes[0])]]))/(2*np.pi)*samplerate

    # clip the freqs so they don't go below zero
    #f = f.clip(0,f.max())

    return f,amp,phase

def get_dispersion_correction(on_scan, off_scan, scanLen=30.0,
                              deltaT=0.1,
                              direc='/media/Disk1/nov14_2015'):
    """
    Gets the linear fits for c_y and c_z 
    given On and Off scans
    """
    bf = BinFile(get_gbtscan_file(direc=direc, scan=on_scan)[0], 
                 number_packets=3000)
    bf.load_sti_cross_correlate(scanLen, deltaT)
    Ron = bf.sti_cc.mean(axis=3)
    Ron = numpy.delete(Ron, bf.bad_inputs, axis=0)
    Ron = numpy.delete(Ron, bf.bad_inputs, axis=1)

    bf = BinFile(get_gbtscan_file(direc=direc, scan=off_scan)[0], 
                 number_packets=3000)
    bf.load_sti_cross_correlate(scanLen, deltaT)
    Roff = bf.sti_cc.mean(axis=3)
    Roff = numpy.delete(Roff, bf.bad_inputs, axis=0)
    Roff = numpy.delete(Roff, bf.bad_inputs, axis=1)
    
    alpha_y = numpy.zeros(bf.num_bins)
    alpha_z = numpy.zeros(bf.num_bins)
    
    for b in range(bf.num_bins):
        onoff = Ron[:, :, b] - Roff[:, :, b]
        Rxy = onoff[:12, 12:24]
        Rxz = onoff[:12, 24:]
        alpha_y[b] = numpy.angle(Rxy.sum())
        alpha_z[b] = numpy.angle(Rxz.sum())
        
    x = numpy.arange(1, bf.num_bins+1)
    c_y = numpy.polyfit(x, numpy.unwrap(alpha_y), 1)
    c_z = numpy.polyfit(x, numpy.unwrap(alpha_z), 1)
    return c_y, c_z

 def plot_cross_angle_grid(self, bf, refpixel='3,3', onoff=None,
                           off=None, 
                           ymin=-3, ymax=3,
                           title=None,
                           hold=False):
     if not hold:
         self.clear()
     refpixel_idx = bf.map_pixel_spec[refpixel]
     if onoff is None:
         cc = bf.sti_cc
     else:
         cc = onoff
     for row in range(2, 8):
         for col in range(1, 9):
             r = (row - 2) * 8
             pl_idx = r + col
             if bf.map_pixel_spec.has_key("%d,%d" % (row, col)):
                 pix = "%d,%d" % (row, col)
                 ax = self.add_subplot(6, 8, pl_idx)
                 spec_idx = bf.map_pixel_spec[pix]
                 if off is None:
                     spec = numpy.unwrap(numpy.angle(cc[spec_idx, refpixel_idx, :, :].mean(axis=1)))
                 else: 
                     spec = numpy.unwrap(numpy.angle(cc[spec_idx, refpixel_idx, :, :].mean(axis=1))/numpy.sqrt(numpy.abs(off[spec_idx, spec_idx, :, :].mean(axis=1)) * numpy.abs(off[refpixel_idx, refpixel_idx, :, :].mean(axis=1))))
                 self.plot(numpy.arange(bf.bin_start, bf.bin_end+1), spec, linestyle='steps-mid', label=pix)
                 self.set_ylim(ymin, ymax)
                 if pl_idx != 1:
                     ax.set_xticklabels([])
                     ax.set_yticklabels([])
             else:
                 print "%d,%d not available" % (row, col)
                 continue        
     self.redraw_plot()
     if title is not None:
         self.set_figtitle("%s" % title)

def diffplot(freq, B, A, B2, A2):
	w, h = sps.freqz(B, A)
	w2, h2 = sps.freqz(B2, A2)

#	h = h - h2
	dabs = abs(h2) / abs(h)
	dphase = np.unwrap(np.angle(h2)) - np.unwrap(np.angle(h))  

	fig = plt.figure()
	plt.title('Difference between digital filter frequency responses')

	ax1 = fig.add_subplot(111)

	plt.plot(w * (freq/np.pi) / 2.0, 20 * np.log10(dabs), 'b')
	plt.ylabel('Amplitude [dB]', color='b')
	plt.xlabel('Frequency [rad/sample]')

	ax2 = ax1.twinx()
	angles = np.unwrap(np.angle(h))
	angles = dphase
	plt.plot(w * (freq/np.pi) / 2.0, angles, 'g')
	plt.ylabel('Angle (radians)', color='g')
	plt.grid()
	plt.axis('tight')
	plt.show()

def makeGnuFig(filename):
    resultmat = np.zeros([len(squid.xaxis), 9])
    S11 = elem.S11
    ydat = measdata.ydat
    resultmat[:, 0] = squid.xaxis
    resultmat[:, 1] = ydat.real
    resultmat[:, 2] = S11.real
    resultmat[:, 3] = ydat.imag
    resultmat[:, 4] = S11.imag
    resultmat[:, 5] = abs(ydat)
    resultmat[:, 6] = abs(S11)
    resultmat[:, 7] = np.unwrap(np.angle(S11), discont=pi)
    resultmat[:, 8] = np.unwrap(np.angle(ydat), discont=pi)
    np.savetxt(filename, resultmat, delimiter='\t')
    # Plot in Gnuplot
    g1 = gp.Gnuplot(persist=1, debug=1)
    g1("plot '" + str(filename) + "' u 1:2 w l t 'Meas.real'")
    g1("replot '" + str(filename) + "' u 1:3 w l t 'Fit.real'")
    g1("replot '" + str(filename) + "' u 1:4 w l t 'Meas.imag'")
    g1("replot '" + str(filename) + "' u 1:5 w l t 'Fit.imag'")
    g2 = gp.Gnuplot(persist=1, debug=1)
    g2("plot '" + str(filename) + "' u 1:6 w l t 'Meas.mag'")
    g2("replot '" + str(filename) + "' u 1:7 w l t 'Fit.mag'")
    g2("replot '" + str(filename) + "' u 1:8 w l t 'Meas.phase'")
    g2("replot '" + str(filename) + "' u 1:9 w l t 'Fit.phase'")
    return

def follow_trajectory(pr2, bodypart2traj):    
    """
    bodypart2traj is a dictionary with zero or more of the following fields: {l/r}_grab, {l/r}_gripper, {l/r_arm}
    We'll follow all of these bodypart trajectories simultaneously
    Also, if the trajectory involves grabbing with the gripper, and the grab fails, the trajectory will abort
    """        
    rospy.loginfo("following trajectory with bodyparts %s", " ".join(bodypart2traj.keys()))
    trajectories = []
    vel_limits = []
    acc_limits = []
    bodypart2inds = {}
    
    n_dof = 0
    name2part = {"l_gripper":pr2.lgrip, 
                 "r_gripper":pr2.rgrip, 
                 "l_arm":pr2.larm, 
                 "r_arm":pr2.rarm}
    
    for (name, part) in name2part.items():
        if name in bodypart2traj:
            traj = bodypart2traj[name]
            if traj.ndim == 1: traj = traj.reshape(-1,1)
            trajectories.append(traj)
            vel_limits.extend(part.vel_limits)
            acc_limits.extend(part.acc_limits)
            bodypart2inds[name] = range(n_dof, n_dof+part.n_joints)
            n_dof += part.n_joints
           
    trajectories = np.concatenate(trajectories, 1)
    #print 'traj orig:'; print trajectories
    #trajectories = np.r_[np.atleast_2d(pr2.get_joint_positions()), trajectories]
    #print 'traj with first:'; print trajectories
    for arm in ['l_arm', 'r_arm']:
        if arm in bodypart2traj:
            part_traj = trajectories[:,bodypart2inds[arm]]
            part_traj[:,4] = np.unwrap(part_traj[:,4])
            part_traj[:,6] = np.unwrap(part_traj[:,6])
    #print 'traj after unwrap:'; print trajectories
    
    vel_limits = np.array(vel_limits)
    acc_limits = np.array(acc_limits)

    times = retiming.retime_with_vel_limits(trajectories, vel_limits/2)
    times_up = np.arange(0,times[-1],.1)
    traj_up = interp2d(times_up, times, trajectories)

    for (name, part) in name2part.items():
        if name in bodypart2traj:
            part_traj = traj_up[:,bodypart2inds[name]]
            if name == "l_gripper" or name == "r_gripper":
                part.follow_timed_trajectory(times_up, part_traj.flatten())
            elif name == "l_arm" or name == "r_arm":
                #print 'following traj for', name, part_traj
                #print '   with velocities'
                vels = kinematics_utils.get_velocities(part_traj, times_up, .001)
                #print vels
                part.follow_timed_joint_trajectory(part_traj, vels, times_up)
    pr2.join_all()    
    return True