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)
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)
# 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)
#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])
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"))
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"))
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))
