def calibrateData(procCmd):
    
#    calData = datAc.pipeAcquisition(procCmd,4,measNr=150)
    calData = datAc.collectForTime(procCmd,5)
    
    # convert to Tesla
#    print "type calData", type(calData)
#    print "type calData[0]",type(calData[0])
    nInd = calData[0]*1e-7
    nMid = calData[1]*1e-7
    nRin = calData[2]*1e-7
    nPin = calData[3]*1e-7
    # reassign the tuple...
    calData = None
    calData = (nInd, nMid, nRin, nPin)
    
        
    # calculating the B-field
    fingerList = [h.phalInd]
    jointList = [h.jointInd]
    sensList = [h.sInd,h.sMid,h.sRin,h.sPin]

    t = np.arange(0,1/2.*np.pi,0.01)
    angles = np.zeros((len(t),3))
    cnt = 0
    for i in t:
        angles[cnt] = np.array([i, 0., 0.])                                
        cnt += 1
    

#    calcB_dip = np.zeros((len(t),3*len(sensList)))
    calcB_cyl = np.zeros((len(t),3*len(sensList)))
    cnt = 0
    for i in angles:
#        calcB_dip[cnt] = modD.cy.angToBm_cy(i,fingerList,sensList,jointList)
        calcB_cyl[cnt] = modC.cy.angToB_cyl(i,fingerList,sensList,jointList)
        cnt += 1

    # fitting to dipole model
    (scaleInd,offInd) = datAc.getScaleOff(calcB_cyl[:,0:3] , calData[0])
    (scaleMid,offMid) = datAc.getScaleOff(calcB_cyl[:,3:6] , calData[1])
    (scaleRin,offRin) = datAc.getScaleOff(calcB_cyl[:,6:9] , calData[2])
    (scalePin,offPin) = datAc.getScaleOff(calcB_cyl[:,9:12], calData[3])
    
    scaleArr = np.array([scaleInd, scaleMid, scaleRin, scalePin])
    offArr = np.array([offInd, offMid, offRin, offPin])
    
    cInd = calData[0]*scaleInd+offInd
    cMid = calData[1]*scaleMid+offMid
    cRin = calData[2]*scaleRin+offRin
    cPin = calData[3]*scalePin+offPin
    
    plo.plotter2d((cInd,cMid,cRin,cPin),("calIndex","calMid","calRin","calPin"))
    plt.figure()
    plo.plotter2d((calcB_cyl[:,0:3],calcB_cyl[:,3:6],calcB_cyl[:,6:9],calcB_cyl[:,9:]),
                  ("simIndex","simMid","simRin","simPin"))

    return (scaleArr, offArr)
예제 #2
0
def calibrateData(procCmd):
    print "cali"


    fingerList = [h.phalInd,h.phalMid,h.phalRin,h.phalPin]
    jointList = [h.jointInd_car,h.jointMid_car,h.jointRin_car,h.jointPin_car]
    sensList = [h.sInd_car,h.sMid_car,h.sRin_car,h.sPin_car]


#    calData = datAc.pipeAcquisition(procCmd,4,measNr=150)
    calData = datAc.collectForTime(procCmd,20)

    t = np.arange(0,1/2.*np.pi,0.01)
    angles = np.zeros((len(t),2*len(sensList)))
    cnt = 0
    for i in t:
        angles[cnt] = np.array([i, 0.,
                                i, 0.,
                                i, 0.,
                                i, 0.])
        cnt += 1

    # calculating the B-field
    fingerList = [h.phalInd,h.phalMid,h.phalRin,h.phalPin]
    jointList = [h.jointInd_car,h.jointMid_car,h.jointRin_car,h.jointPin_car]
    sensList = [h.sInd_car,h.sMid_car,h.sRin_car,h.sPin_car]

    calcB_dip = np.zeros((len(t),3*len(sensList)))
    calcB_cyl = np.zeros((len(t),3*len(sensList)))
    cnt = 0
    for i in angles:
        calcB_dip[cnt] = modD.cy.angToBm_cy(i,fingerList,sensList,jointList)
        calcB_cyl[cnt] = modC.cy.angToB_cyl(i,fingerList,sensList,jointList)
        cnt += 1

    # fitting to dipole model
    (scaleInd,offInd) = datAc.getScaleOff(calcB_dip[:,0:3] , calData[0])
    (scaleMid,offMid) = datAc.getScaleOff(calcB_dip[:,3:6] , calData[1])
    (scaleRin,offRin) = datAc.getScaleOff(calcB_dip[:,6:9] , calData[2])
    (scalePin,offPin) = datAc.getScaleOff(calcB_dip[:,9:12], calData[3])

    # fitting to cylindrical model
    # (scaleInd,offInd) = datAc.getScaleOff(calcB_cyl[:,0:3] , calData[0])
    # (scaleMid,offMid) = datAc.getScaleOff(calcB_cyl[:,3:6] , calData[1])
    # (scaleRin,offRin) = datAc.getScaleOff(calcB_cyl[:,6:9] , calData[2])
    # (scalePin,offPin) = datAc.getScaleOff(calcB_cyl[:,9:12], calData[3])

    scaleArr = np.array([scaleInd, scaleMid, scaleRin, scalePin])
    offArr = np.array([offInd, offMid, offRin, offPin])

    return (scaleArr, offArr)
예제 #3
0
# all the start and end states for the corresponding datasets...
startL = [480,728,99 ,50,   180,40,120,120,120,120,140,130,220,140,110]
endL =   [529,799,185,90,   280,80,180,160,180,160,180,175,300,180,160]

for i in range(4,5,1):      
    sstring = 'set'+str(i)
    
    (tim, s1, s2, s3, s4) = datAc.readMag("../datasets/evalSets/"+sstring+"_mag")
    (tLeap, lInd, lMid, lRin, lPin) = datAc.readLeap("../datasets/evalSets/"+sstring+"_leap")
    lInd_re = sf.resampleLeap((tLeap,lInd),tim)[0]
    
    ''' fitting measurements ''' 
#    print "fitting for 90"
    start = startL[i-1]
    end = endL[i-1]
    (scale, off) = datAc.getScaleOff(b90_A[:,:3],s1[start:end]) 
    s1_fit = s1*scale+off
    
    (scale, off) = datAc.getScaleOff(b90_A[:,3:6],s2[start:end]) 
    s2_fit = s2*scale+off
    
    (scale, off) = datAc.getScaleOff(b90_A[:,6:9],s3[start:end]) 
    s3_fit = s3*scale+off
    
    (scale, off) = datAc.getScaleOff(b90_A[:,9:],s4[start:end]) 
    s4_fit = s4*scale+off
    
    ## combining everything again...
    s1_fit = np.append(s1_fit,np.append(s2_fit,np.append(s3_fit,s4_fit,1),1),1)       
    
    
예제 #4
0
#end = 124
#p = np.zeros((4,124-32,3))

p[0] = d[0][start:end]
p[1] = d[1][start:end]
p[2] = d[2][start:end]
p[3] = d[3][start:end]

#p = d

#scaled0 = datAc.scaleMeasurements(b_dip0,p[0])
#scaled1 = datAc.scaleMeasurements(b_dip1,p[1])
#scaled2 = datAc.scaleMeasurements(b_dip2,p[2])
#scaled3 = datAc.scaleMeasurements(b_dip3,p[3])

(scale0, off0) = datAc.getScaleOff(b_dip0, p[0])
scaled05 = p[0]*np.array([1.20967052,  1.26494698 , 1.13013986])
off05 = np.array([b_dip0[0][0]-scaled05[0][0],
                  b_dip0[0][1]-scaled05[0][1],
                  b_dip0[0][2]-scaled05[0][2]])
scaled05 += off05    

scaled0 = p[0]*scale0 + off0

unscaled = p[0]
offUn = np.array([b_dip0[0][0]-unscaled[0][0],
                  b_dip0[0][1]-unscaled[0][1],
                  b_dip0[0][2]-unscaled[0][2]])
unscaled += offUn                  

#(scale1, off1, scaled1) = datAc.getScaleOff(b_dip1, p[1])
예제 #5
0
angles = np.zeros((len(t),2*len(sensList)))
cnt = 0
for i in t:
    angles[cnt] = np.array([i, 0.,
                            i, 0.,
                            i, 0.,
                            i, 0.])
    cnt += 1



b_dip = np.zeros((len(t),3*len(sensList)))
b_cyl = np.zeros((len(t),3*len(sensList)))
cnt = 0
for i in angles:
    b_dip[cnt] = modD.cy.angToBm_cy(i,fingerList,sensList,jointList)    
    b_cyl[cnt] = modC.angToB_cyl(i,fingerList,sensList,jointList)    
    cnt += 1

#    caliPos = calcBInd_m[0]

(scale, off) = datAc.getScaleOff(b_dip, meas)

meas_fit = meas * scale + off

plo.plotter2d((b_dip[:,:3], b_dip[:,3:6], b_dip[:,6:9], b_dip[:,9:]),
              ("dipole index","dipole middle","dipole ring","dipole pinky"))
plo.plotter2d((meas_fit[:,:3], meas_fit[:,3:6], meas_fit[:,6:9], meas_fit[:,9:]),
              ("meas_fit index","meas_fit middle","meas_fit ring","meas_fit pinky"))  
plo.plotter2d((meas[:,:3], meas[:,3:6], meas[:,6:9], meas[:,9:]),
              ("meas_raw index","meas_raw middle","meas_raw ring","meas_raw pinky"))                
예제 #6
0
    
    
bMid = np.zeros((len(t), 3*len(sensList_dip)))    
bRin = np.zeros((len(t), 3*len(sensList_dip)))    
cnt = 0
for i in range(len(t)):
#    b_cyl[i] = modC.cy.angToB_cyl(angles_cyl[i],fingList_cyl,sensList_cyl,jointList_cyl)    # simulating cylindrical model
    bMid[i] = modD.cy.angToBm_cy(angles1[i],fingList_dip,sensList_dip,jointList_dip)              # simulating dipole model
    bRin[i] = modD.cy.angToBm_cy(angles2[i],fingList_dip,sensList_dip,jointList_dip)              # simulating dipole model
    
    
b_meas = datAc.textAcquisition("160131_allMid2")    
b_meas *= 0.01
print "-----------middle----------"
print "INDEX"    
(scale1, off1) = datAc.getScaleOff(bMid[:,0:3],b_meas[0])    
b_ind = b_meas[0]*scale1+off1
print "MIDDLE"
#(scale2, off2) = datAc.getScaleOff(bMid[:,3:6],b_meas[1])    
#b_mid = b_meas[0]*scale2+off2
print "RING"
(scale3, off3) = datAc.getScaleOff(bMid[:,6:9],b_meas[2])    
b_rin = b_meas[2]*scale3+off3
print "PINKY"
(scale4, off4) = datAc.getScaleOff(bMid[:,9:],b_meas[3])    
b_pin = b_meas[3]*scale4+off4

#plo.plotter2d((bMid[:,0:3],b_ind,b_meas[0]),("sim index","index fit","raw"))
#plo.plotter2d((bMid[:,3:6],b_mid,b_meas[1]),("sim middle","middle fit","raw"))
#plo.plotter2d((bMid[:,6:9],b_rin,b_meas[2]),("sim ring","ring fit","raw"))
#plo.plotter2d((bMid[:,9:], b_pin,b_meas[3]),("sim pinky","pinky fit","raw"))
예제 #7
0
        
    cnt += 1
    
b = np.zeros((len(t), 3*len(sensList)))    

cnt = 0
for i in range(len(t)):    
    b[i] = modD.cy.angToBm_cy(angles[i],fingList,sensList,jointList)              # simulating dipole model



''' fitting '''
d = datAc.textAcquisition("160129_mid3")[1]
d *= 0.01

(scale, off) = datAc.getScaleOff(b,d)

d_fit = d*scale+off

plo.plotter2d((b,d_fit,d),("sim","fitted","raw"),shareAxis=False)


''' estimation '''

P = np.eye(2)
# process noise covariance matrix (2x2)
Q = np.diag([1e+2, 1e-2])                
# measurement noise covariance matrix (3x3)                
R = np.diag([1e+1, 1e+1, 1e+1])    

estAngK = np.zeros((len(d_fit)+1,2))