def exp_jiao_environment():
map = ExpSetting.getBasicMap()
fig = MapFigure(map)
fig.drawMapByFeatures()
fig.setXYLabel(u'距离/mm', u'距离/mm')
fig.setXYLim(0, 2500, 0, 3200)
fig.showFigure()
def drawSensorLocErr():
fig = MapFigure(Map2D(200, 200))
l1 = fig.drawErrorBar(x, yLaser, eLaser, 'm-s')
l2 = fig.drawErrorBar(x, ySonic, eSonic, 'b--o')
fig.setXYLabel(u'栅格单元/cm', u'平均定位误差/cm')
fig.setXYLim(0, 70, 0, 35)
plt.legend([l1, l2], [u'LaserRad', u'SonicSen'], loc='upper right')
fig.showFigure()
def sim1(init_val):
num = 60
sam = 60
seqX = PFSim.getSeqX(init_val, num)
seqO = PFSim.getSeqO(seqX)
rans = PFSim.genNormalRan(num, sam)
# SIS
seqR = [init_val]
timeSIS = time.clock()
pset = ParticleSet(np.linspace(-10, 10, 400))
for i in range(1, len(seqX)):
pset.samplingSIS(sam)
pset.predictSeq(i, rans[i])
pset.updateSeq(seqO[i])
seqR.append(pset.getCurrentVal())
timeSIS = time.clock() - timeSIS
# LVS
seqLVS = [init_val]
timeLVS = time.clock()
pset = ParticleSet(np.linspace(-10, 10, 400))
for i in range(1, len(seqX)):
pset.samplingLVS(sam)
pset.predictSeq(i, rans[i])
pset.updateSeq(seqO[i])
seqLVS.append(pset.getCurrentVal())
timeLVS = time.clock() - timeLVS
# VIS
seqVIS = [init_val]
timeVIS = time.clock()
pset = ParticleSet(np.linspace(-10, 10, 400))
for i in range(1, len(seqX)):
pset.samplingVIS(sam)
pset.predictSeq(i, rans[i])
pset.updateSeq(seqO[i])
seqVIS.append(pset.getCurrentVal())
timeVIS = time.clock() - timeVIS
fig = MapFigure(Map2D(200, 200))
index = np.arange(0, num, 1)
print len(index), len(seqX)
print PFSim.getRMSE(seqX, seqR)
print PFSim.getRMSE(seqX, seqLVS)
print PFSim.getRMSE(seqX, seqVIS)
print timeSIS, timeLVS, timeVIS
l1 = fig.drawRadarPoints(index, seqX, 'ro-')
l2 = fig.drawRadarPoints(index, seqR, 'm<:', 1.5)
l3 = fig.drawRadarPoints(index, seqLVS, 'c<:', 1.5)
l4 = fig.drawRadarPoints(index, seqVIS, 'g*--', 1.5)
plt.legend([l1, l2, l3, l4], [u'True', u'SIS', 'LVS', 'VIS'],
loc='upper right')
fig.setXYLabel(u'数据编号', u'距离/mm')
#print seqR
fig.showFigure()
def drawPNum():
fig = MapFigure(Map2D(200, 200))
l1 = fig.drawRadarPoints(n_x, n_fix1, 'm-s')
l2 = fig.drawRadarPoints(n_x, n_fix2, 'b--*')
l3 = fig.drawRadarPoints(n_x, n_ada, 'g:o', 1.5)
plt.legend([l1, l2, l3], [u'FIX1', u'FIX2', 'ADA'], loc='upper right')
fig.setXYLabel(u'数据编号', u'粒子数/个')
fig.showFigure()
def exp_jiao_radardata(radar_pos):
map = ExpSetting.getBasicMap()
fig = MapFigure(map)
radar = ExpSetting.getBasicRadar()
radar_data = radar.getMeasureData(map, radar_pos)
rag = DBSCAN.getFeatureRange(radar_data, radar_pos)
colors = ['b.', 'r.', 'g.']
xs = [x[0] for x in radar_data]
ys = [x[1] for x in radar_data]
for i in range(len(rag)):
s = rag[i][0]
e = rag[i][1]
fig.drawRadarPoints(xs[s:e], ys[s:e], colors[i])
fig.setXYLabel(u'距离/mm', u'距离/mm')
fig.setXYLim(0, 2500, 0, 3200)
fig.showFigure()
def drawParticleSize():
y = []
for i in range(len(x)):
y.append(PFSim.genKLDNum(6400 / x[i]))
fig = MapFigure(Map2D(200, 200))
l = fig.drawRadarPoints(x, y, 'bo-')
fig.setXYLabel(u'栅格单元/cm', u'平均采样数/个')
#plt.legend([l], [u'KLD'], loc = 'upper right')
fig.showFigure()
def sim2(init_val):
num = 20
sam = 60
seqX = PFSim.getSeqX(init_val, num)
seqO = PFSim.getSeqO(seqX)
rans = PFSim.genNormalRan(num, sam)
# SIS-KLD-FIX
seqFIX1 = [init_val]
timeSIS = time.clock()
nums1 = []
pset = ParticleSet(np.linspace(-20, 20, 4000))
for i in range(1, len(seqX)):
toSam = PFSim.genKLDNum(max(1, pset.getOccRoom() / 80))
nums1.append(toSam)
pset.samplingSIS(toSam)
pset.predictSeq(i, rans[i])
pset.updateSeq(seqO[i])
seqFIX1.append(pset.getCurrentVal())
timeSIS = time.clock() - timeSIS
# SIS-KLD-FIX
seqFIX2 = [init_val]
timeFIX2 = time.clock()
nums2 = []
pset = ParticleSet(np.linspace(-20, 20, 4000))
for i in range(1, len(seqX)):
toSam = PFSim.genKLDNum(max(1, pset.getOccRoom() / 240))
nums2.append(toSam)
pset.samplingSIS(toSam)
pset.predictSeq(i, rans[i])
pset.updateSeq(seqO[i])
seqFIX2.append(pset.getCurrentVal())
timeFIX2 = time.clock() - timeFIX2
# SIS-KLD-ADA
seqADA = [init_val]
timeADA = time.clock()
nums3 = []
pset = ParticleSet(np.linspace(-20, 20, 4000))
for i in range(1, len(seqX)):
ori = pset.getOccRoom() / 200
toSam = PFSim.genKLDNum(min(7, ori))
nums3.append(toSam)
pset.samplingSIS(toSam)
pset.predictSeq(i, rans[i])
pset.updateSeq(seqO[i])
seqADA.append(pset.getCurrentVal())
timeADA = time.clock() - timeADA
fig = MapFigure(Map2D(200, 200))
index = np.arange(0, num, 1)
print len(index), len(seqX)
print nums1
print nums2
print nums3
print timeSIS, PFSim.getRMSE(seqX, seqFIX1)
print timeFIX2, PFSim.getRMSE(seqX, seqFIX2)
print timeADA, PFSim.getRMSE(seqX, seqADA)
l1 = fig.drawRadarPoints(index, seqX, 'ro-')
l2 = fig.drawRadarPoints(index, seqFIX1, 'm<:', 1.5)
l3 = fig.drawRadarPoints(index, seqFIX2, 'bd:', 1.5)
l4 = fig.drawRadarPoints(index, seqADA, 'g*--', 1.5)
plt.legend([l1, l2, l3, l4], [u'True', u'FIX1', 'FIX2', 'ADA'],
loc='upper right')
fig.setXYLabel(u'数据编号', u'距离/mm')
#print seqR
fig.showFigure()
radar_data = radar.getMeasureData(map, radar_pos)
bps = DBSCAN.getBreakPoints(radar_data, radar_pos)
rag = DBSCAN.getFeatureRange(radar_data, radar_pos)
for i in range(len(radar_data)):
print i, radar_data[i]
print bps
print rag
p1 = Split.getSingleSplitPoint(radar_data, rag[0])
print p1
p2s = Split.getAroundIdx(radar_data, p1, 25)
xs = [x[0] for x in radar_data]
ys = [x[1] for x in radar_data]
fig = MapFigure(map)
fig.drawMapByFeatures()
colors = ['b.', 'r.', 'g.']
for i in range(len(rag)):
s = rag[i][0]
e = rag[i][1]
fig.drawRadarPoints(xs[s:e], ys[s:e], colors[i])
fig.drawRadarPoints(xs[p1], ys[p1], 'wo')
fig.drawRadarPoints(xs[p2s[0]], ys[p2s[0]], 'wo')
fig.drawRadarPoints(xs[p2s[1]], ys[p2s[1]], 'wo')
fig.setXYLim(0, 2000, 0, 2000)
fig.showFigure()
'''
xs = [[25, 825], [975, 1575]]
ys = [[25, 85], [235, 285], [435, 465], [615, 665], [815, 885]]
def genRandPos(x, y, w, h, num):
ran1 = RandomNum.createRandom(num) * w + x
ran2 = RandomNum.createRandom(num) * h + y
return ran1, ran2
w = 1600
h = 900
map = ExpSetting.getBasicMap(w, h, Feas.fea2)
fig = MapFigure(map)
fig.drawMapByFeatures()
# 主通道
#ran1, ran2 = genRandPos(825, 25, 150, 850, 300)
# 搜索c
#ran1, ran2 = genRandPos(825, 210, 150, 100, 50)
#
ran1, ran2 = genRandPos(825, 690, 150, 80, 50)
fig.drawRadarPoints(ran1, ran2, 'b.')
'''
for i in range (len(xs)):
for j in range(len(ys)):
ran1, ran2 = genRandPos(xs[i][0], ys[j][0], xs[i][1] - xs[i][0], ys[j][1] - ys[j][0], 120)
fig.drawRadarPoints(ran1, ran2, 'b.')
'''
def drawMeasurePoint(radar_data, rag, p=[0, 0]):
# 绘图
xs = [x[0] for x in radar_data]
ys = [x[1] for x in radar_data]
fig = MapFigure(map)
fig.drawMapByFeatures()
colors = ['b.', 'r.', 'g.', 'b.', 'r.', 'g.', 'b.', 'r.', 'g.']
for i in range(len(rag)):
s = rag[i][0]
e = rag[i][1]
fig.drawRadarPoints(xs[s:e], ys[s:e], colors[i])
fig.drawRadarPoints([p[0]], [p[1]], 'wo')
fig.setXYLim(0, 2000, 0, 3000)
fig.showFigure()
def exp_jiao_drawEstimate(p1, p2, p3, y1, y2):
xs = np.arange(1, 11)
map = ExpSetting.getBasicMap()
fig = MapFigure(map)
l1 = fig.drawRadarPoints(xs, p3, 'ro-')
l2 = fig.drawRadarPoints(xs, p1, 'g--<')
l3 = fig.drawRadarPoints(xs, p2, 'm:d')
plt.legend([l1, l2, l3], [u'AAAAA', u'B', 'C'], loc='upper right')
fig.setXYLabel(u'数据编号', u'距离/mm')
fig.setXYLim(0, 11, y1, y2)
fig.showFigure()
def exp_jiao_drawErrRange():
dats = [[1977, 2007], [1982, 2012], [1993, 2002], [1979,
2006], [1994, 2023],
[1992, 2016], [1992, 2015], [1996, 2017], [1983, 2003],
[1996, 2014]]
x = np.arange(1, 11, 1)
y = []
e = []
for i in range(len(dats)):
y.append((dats[i][0] + dats[i][1]) / 2.0)
e.append(dats[i][1] - y[i])
map = ExpSetting.getBasicMap()
fig = MapFigure(map)
fig.drawErrorBar(x, y, e, 'k-o')
fig.drawRadarPoints(np.arange(0, 12), [2000] * 12, 'r--')
fig.setXYLabel(u'数据编号', u'距离/mm')
fig.setXYLim(0, 11, 1950, 2050)
fig.showFigure()