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()