def runVB(self,prior,softClassNum): #For the one dimensional case only post = GM(); weight = self.weights; bias = self.bias; alpha = self.alpha; zeta_c = self.zeta_c; for g in prior.Gs: prevLogCHat = -1000; count = 0; while(count < 100000): count = count+1; [mean,var,yc,yc2] = self.Estep(weight,bias,g.mean,g.var,alpha,zeta_c,softClassNum = softClassNum); [zeta_c,alpha] = self.Mstep(len(weight),yc,yc2,zeta_c,alpha,steps = 100); logCHat = self.calcCHat(g.mean,g.var,mean,var,alpha,zeta_c,yc,yc2,mod=softClassNum); if(abs(prevLogCHat - logCHat) < 0.00001): break; else: prevLogCHat = logCHat; post.addG(Gaussian(mean,var,g.weight*np.exp(logCHat).tolist()[0][0])) return post;
def testRectangleModel(): pz = Softmax(); pz.buildRectangleModel([[2,2],[3,4]],1); #print('Plotting Observation Model'); #pz.plot2D(low=[0,0],high=[10,5],vis=True); prior = GM(); for i in range(0,10): for j in range(0,5): prior.addG(Gaussian([i,j],[[1,0],[0,1]],1)); # prior.addG(Gaussian([4,3],[[1,0],[0,1]],1)); # prior.addG(Gaussian([7,2],[[4,1],[1,4]],3)) prior.normalizeWeights(); dela = 0.1; x, y = np.mgrid[0:10:dela, 0:5:dela] fig,axarr = plt.subplots(6); axarr[0].contourf(x,y,prior.discretize2D(low=[0,0],high=[10,5],delta=dela)); axarr[0].set_title('Prior'); titles = ['Inside','Left','Right','Up','Down']; for i in range(0,5): post = pz.runVBND(prior,i); c = post.discretize2D(low=[0,0],high=[10,5],delta=dela); axarr[i+1].contourf(x,y,c,cmap='viridis'); axarr[i+1].set_title('Post: ' + titles[i]); plt.show();
def generate(self, s, a): #sprime = np.random.choice([i for i in range(0,self.model.N)],p=self.model.px[a][s]); #tmpGM = GM((np.array(s) + np.array(self.model.delA)).T.tolist(),self.model.delAVar,1); tmpGM = GM() tmpGM.addG( Gaussian((np.array(s) + np.array(self.model.delA[a])).tolist(), self.model.delAVar, 1)) sprime = tmpGM.sample(1)[0] ztrial = [0] * len(self.model.pz) for i in range(0, len(self.model.pz)): ztrial[i] = self.model.pz[i].pointEval(sprime) z = ztrial.index(max(ztrial)) reward = self.model.r[a].pointEval(s) ''' if(a == 0 and s > 13): reward = 10; elif(a==1 and s<13): reward = 10; elif(a == 2 and s==13): reward = 100; else: reward = -10; ''' return [sprime, z, reward]
def beliefUpdate(self, b, a, o, maxMix=10): btmp = GM() for i in self.pz[o].Gs: for j in b.Gs: tmp = mvn.pdf( np.add(np.matrix(j.mean), np.matrix(self.delA[a])).tolist(), i.mean, self.covAdd(self.covAdd(i.var, j.var), self.delAVar)) #print(i.weight,j.weight,tmp); w = i.weight * j.weight * tmp.tolist() sig = (np.add( np.matrix(i.var).I, np.matrix(self.covAdd(j.var, self.delAVar)).I)).I.tolist() #sstmp = np.matrix(i.var).I*np.transpose(i.mean) + np.matrix(self.covAdd(j.var + self.delAVar)).I*np.transpose(np.add(np.matrix(j.mean),np.matrix(delA[a]))); sstmp1 = np.matrix(i.var).I * np.transpose(np.matrix(i.mean)) sstmp2 = np.matrix(self.covAdd(j.var, self.delAVar)).I sstmp21 = np.add(np.matrix(j.mean), np.matrix(self.delA[a])) sstmp3 = sstmp1 + sstmp2 * np.transpose(sstmp21) smean = np.transpose(sig * sstmp3).tolist()[0] btmp.addG(Gaussian(smean, sig, w)) btmp = btmp.kmeansCondensationN(maxMix) #btmp.condense(maxMix); btmp.normalizeWeights() return btmp
def beliefUpdate(b, a, o, pz): btmp = GM() adelA = [-1, 1, 0] adelAVar = 0.5 for obs in pz[o].Gs: for bel in b.Gs: sj = np.matrix(bel.mean).T si = np.matrix(obs.mean).T delA = np.matrix(adelA[a]).T sigi = np.matrix(obs.var) sigj = np.matrix(bel.var) delAVar = np.matrix(adelAVar) weight = obs.weight * bel.weight weight = weight * mvn.pdf( (sj + delA).T.tolist()[0], si.T.tolist()[0], np.add(sigi, sigj, delAVar)) var = (sigi.I + (sigj + delAVar).I).I mean = var * (sigi.I * si + (sigj + delAVar).I * (sj + delA)) weight = weight.tolist() mean = mean.T.tolist()[0] var = var.tolist() btmp.addG(Gaussian(mean, var, weight)) btmp.normalizeWeights() btmp.condense(1) btmp.normalizeWeights() return btmp
def stateLWISUpdate(self): cp=self.prevPoses[-1]; prev = self.prevPoses[-2]; theta = np.arctan2([cp[1]-prev[1]],[cp[0]-prev[0]]); #print(theta); radius = self.ROBOT_VIEW_RADIUS; points = [[cp[0]-radius,cp[1]-radius],[cp[0]+radius,cp[1]-radius],[cp[0]+radius,cp[1]+radius],[cp[0]-radius,cp[1]+radius]]; soft = Softmax() soft.buildPointsModel(points,steepness=1); #soft.buildTriView(pose = [cp[0],cp[1],theta],length=10,steepness=5); change = False; post = GM(); for g in self.belief: if(distance(cp,g.mean) > self.ROBOT_VIEW_RADIUS+5): post.addG(g); else: change = True; tmp = soft.lwisUpdate(g,0,20,inverse=True); #self.bounds = {'low':[0,0],'high':[437,754]} tmp.mean[0] = max(self.bounds['low'][0]+1,tmp.mean[0]); tmp.mean[1] = max(self.bounds['low'][1]+1,tmp.mean[1]); tmp.mean[0] = min(self.bounds['high'][0]-1,tmp.mean[0]); tmp.mean[1] = min(self.bounds['high'][1]-1,tmp.mean[1]); post.addG(tmp); self.belief = post; self.belief.normalizeWeights(); return change;
def transformCartToPol(bcart, offset=[0, 0]): bpol = GM() for g in bcart: m = g.mean m1 = [0, 0] m1[0] = m[0] - offset[0] m1[1] = m[1] - offset[1] mPrime = [np.sqrt(m1[0]**2 + m1[1]**2), np.arctan2(m1[1], m1[0])] if (m1[0]**2 + m1[1]**2 == 0): m1[0] = 0.0001 m1[1] = 0.0001 J11 = m1[0] / np.sqrt(m1[0]**2 + m1[1]**2) J12 = m1[1] / np.sqrt(m1[0]**2 + m1[1]**2) J21 = -m1[1] / (m1[0]**2 + m1[1]**2) J22 = m1[0] / (m1[0]**2 + m1[1]**2) JCarPol = np.matrix([[J11, J12], [J21, J22]]) var = np.matrix(g.var) varPrime = (JCarPol * var * JCarPol.T).tolist() bpol.addG(Gaussian(mPrime, varPrime, g.weight)) return bpol
def beliefUpdate(self,b,a,o): btmp = GM(); for obs in self.pz[o].Gs: for bel in b.Gs: sj = np.matrix(bel.mean).T; si = np.matrix(obs.mean).T; delA = np.matrix(self.delA[a]).T; sigi = np.matrix(obs.var); sigj = np.matrix(bel.var); delAVar = np.matrix(self.delAVar); weight = obs.weight*bel.weight; weight = weight*mvn.pdf((sj+delA).T.tolist()[0],si.T.tolist()[0],np.add(sigi,sigj,delAVar)); var = (sigi.I + (sigj+delAVar).I).I; mean = var*(sigi.I*si + (sigj+delAVar).I*(sj+delA)); weight = weight.tolist(); mean = mean.T.tolist()[0]; var = var.tolist(); btmp.addG(Gaussian(mean,var,weight)); btmp.normalizeWeights(); btmp = btmp.kmeansCondensationN(self.maxMix); #btmp.condense(maxMix); btmp.normalizeWeights(); return btmp;
def assignRooms(self, belief): #1. partition means into separate GMs, 1 for each room allBels = [] allBounds = [] for room in self.map_.rooms: tmp = GM() tmpw = 0 allBounds.append([ self.map_.rooms[room]['min_x'], self.map_.rooms[room]['min_y'], self.map_.rooms[room]['max_x'], self.map_.rooms[room]['max_y'] ]) for g in belief: m = [g.mean[2], g.mean[3]] # if mean is inside the room if (m[0] < self.map_.rooms[room]['max_x'] and m[0] > self.map_.rooms[room]['min_x'] and m[1] < self.map_.rooms[room]['max_y'] and m[1] > self.map_.rooms[room]['min_y']): tmp.addG(deepcopy(g)) tmpw += g.weight if (tmp.size == 0): centx = (self.map_.rooms[room]['max_x'] + self.map_.rooms[room]['min_x']) / 2 centy = (self.map_.rooms[room]['max_y'] + self.map_.rooms[room]['min_y']) / 2 var = np.identity(4).tolist() tmp.addG(Gaussian([0, 0, centx, centy], var, 0.0001)) allBels.append(tmp) return allBels
def testMakeMap(): translator = POMDPTranslator() b = GM() b.addG(Gaussian([3, 2, 1, 0], np.identity(4).tolist(), 1)) translator.makeBeliefMap(b, [0, 0, 0])
def beliefUpdate(self, b, a, o, mod): btmp = GM() for obs in mod.pz[o].Gs: for bel in b.Gs: sj = np.matrix(bel.mean).T si = np.matrix(obs.mean).T delA = np.matrix(mod.delA[a]).T sigi = np.matrix(obs.var) sigj = np.matrix(bel.var) delAVar = np.matrix(mod.delAVar) weight = obs.weight * bel.weight weight = weight * mvn.pdf( (sj + delA).T.tolist()[0], si.T.tolist()[0], np.add(sigi, sigj, delAVar)) var = (sigi.I + (sigj + delAVar).I).I mean = var * (sigi.I * si + (sigj + delAVar).I * (sj + delA)) weight = weight.tolist() mean = mean.T.tolist()[0] var = var.tolist() btmp.addG(Gaussian(mean, var, weight)) btmp.normalizeWeights() btmp = btmp.kmeansCondensationN(1) #btmp.condense(maxMix); btmp.normalizeWeights() return btmp
def testGeneralModel(): pz = Softmax() pz.buildGeneralModel(2, 4, [[1, 0], [2, 0], [3, 0]], np.matrix([-1, 1, -1, 1, 1, -1, 0, -1, -1]).T) #print('Plotting Observation Model'); #pz.plot2D(low=[0,0],high=[10,5],vis=True); prior = GM() for i in range(0, 10): for j in range(0, 5): prior.addG(Gaussian([i, j], [[1, 0], [0, 1]], 1)) # prior.addG(Gaussian([4,3],[[1,0],[0,1]],1)); # prior.addG(Gaussian([7,2],[[4,1],[1,4]],3)) prior.normalizeWeights() dela = 0.1 x, y = np.mgrid[0:10:dela, 0:5:dela] fig, axarr = plt.subplots(5) axarr[0].contourf(x, y, prior.discretize2D(low=[0, 0], high=[10, 5], delta=dela)) axarr[0].set_title('Prior') titles = ['Inside', 'Left', 'Right', 'Down'] for i in range(0, 4): post = pz.runVBND(prior, i) c = post.discretize2D(low=[0, 0], high=[10, 5], delta=dela) axarr[i + 1].contourf(x, y, c, cmap='viridis') axarr[i + 1].set_title('Post: ' + titles[i]) plt.show()
def backup(als, modes, delA, delAVar, pz, r, maxMix, b): newAls = [[[0 for i in range(0, len(pz))] for j in range(0, len(delA[0]))] for k in range(0, len(als))] for i in range(0, len(als)): for j in range(0, len(delA[0])): for k in range(0, len(pz)): newAls[i][j][k] = GM() for h in modes: tmpGM = als[i].GMProduct(pz[j]) mean = tmpGM.getMeans() for l in range(0, len(mean)): mean[l][0] -= delA[modes.index(h)][j] mean[l] = mean[l][0] var = tmpGM.getVars() for l in range(0, len(var)): var[l][0][0] += delAVar var[l] = var[l][0][0] weights = tmpGM.getWeights() tmpGM2 = GM() for l in range(0, len(mean)): tmpGM2.addG(Gaussian(mean[l], var[l], weights[l])) #tmpGM2 = GM(mean,var,tmpGM.getWeights()); newAls[i][j][k].addGM(tmpGM2.GMProduct(h)) bestVal = -10000000000 bestAct = 0 bestGM = [] for a in range(0, len(delA[0])): suma = GM() for o in range(0, len(pz)): suma.addGM(newAls[np.argmax([ continuousDot(newAls[j][a][o], b) for j in range(0, len(newAls)) ])][a][o]) suma.scalerMultiply(0.9) suma.addGM(r[a]) for g in suma.Gs: if (isinstance(g.mean, list)): g.mean = g.mean[0] g.var = g.var[0][0] suma = suma.kmeansCondensationN(k=maxMix, lowInit=-20, highInit=20) tmp = continuousDot(suma, b) #print(a,tmp); if (tmp > bestVal): bestAct = a bestGM = copy.deepcopy(suma) bestVal = tmp bestGM.action = bestAct return bestGM
def createRandomMixture(size, dims=2): testMix = GM() for i in range(0, size): testMix.addG(sampleWishart(dims)) testMix.normalizeWeights() #testMix.clean(); return testMix
def cutGMTo2D(self, mix, dims=[2, 3]): newer = GM() for g in mix: newer.addG( Gaussian([g.mean[dims[0]], g.mean[dims[1]]], [[g.var[dims[0]][dims[0]], g.var[dims[0]][dims[1]]], [g.var[dims[1]][dims[0]], g.var[dims[1]][dims[1]]]], g.weight)) return newer
def testBeliefUpdate(): translator = POMDPTranslator() b = GM() b.addG(Gaussian([3, 2, 1, 0], np.identity(4).tolist(), 1)) bcut = cutGMTo2D(b, dims=[0, 1]) bcut.plot2D(low=[0, 0], high=[10, 5]) b = translator.beliefUpdate(b, 2, [[8, 5]]) bcut = cutGMTo2D(b, dims=[0, 1]) bcut.plot2D(low=[0, 0], high=[10, 5])
def runVBND(self, prior, softClassNum): #For the N dimensional Case #Note: Cannot run 1D post = GM() for g in prior.Gs: [mu, var, logCHat] = self.vb_update(softClassNum, g.mean, g.var) mu = mu.tolist() var = var.tolist() post.addG(Gaussian(mu, var, g.weight * np.exp(logCHat))) return post
def testGetNextPose(): args = ['PolicyTranslator.py','-n','D2Diffs','-r','True','-a','99','-g','False']; a = PolicyTranslator(args); b = GM(); b.addG(Gaussian([3,3],[[1,0],[0,1]],1)); x = [1,1]; obs = [2,2,2,2,2,2,2,0,0,0,0,0]; for i in range(0,len(obs)): [b,x] = a.getNextPose(b,obs[i],x); print(b.findMAPN()); '''
def testRolloutPolicy(): args = ['PolicyTranslator.py','-n','D2Diffs','-r','True','-a','1','-g','False']; a = PolicyTranslator(args); args2 = ['PolicyTranslator.py','-n','D2DiffsSoftmax','-r','True','-s','True','-a','1','-g','False']; c = PolicyTranslator(args2); b = GM(); b.addG(Gaussian([0,3],[[1,0],[0,1]],1)); print(a.getAction(b),a.getRolloutReward(b,1)); print(c.getAction(b),c.getRolloutReward(b,1));
def beliefUpdateSoftmax(self,b,a,o): btmp = GM(); btmp1 = GM(); for j in b.Gs: mean = (np.matrix(j.mean) + np.matrix(self.delA[a])).tolist()[0]; var = (np.matrix(j.var) + np.matrix(self.delAVar)).tolist(); weight = j.weight; btmp1.addG(Gaussian(mean,var,weight)); btmp = self.pz2.runVBND(btmp1,o); #btmp.condense(maxMix); btmp = btmp.kmeansCondensationN(self.maxMix); btmp.normalizeWeights(); return btmp;
def runSingleSim(self,simSteps = 100,greedySim = False): #Run a simulation #Randomly choose initial state inPose = [0 for i in range(0,len(self.delA[0]))]; for j in range(0,len(self.delA[0])): inPose[j] = random.random()*self.bounds[j][1] + self.bounds[j][0]; inBel = GM(); inBel.addG(Gaussian(inPose,np.eye(len(self.delA[0])).tolist(),1)); #run simulation [allB,allX,allXInd,allAct,allReward] = self.simulate(initialPose = inPose,initialBelief=inBel,numSteps = simSteps,greedy=greedySim); #Show Results fig,ax = plt.subplots(1,sharex=True); for i in range(0,len(allB)): [x,y,c] = allB[i].plot2D(low = [self.bounds[0][0],self.bounds[1][0]],high = [self.bounds[0][1],self.bounds[1][1]],vis = False); ax.cla() ax.contourf(x,y,c,cmap ='viridis'); ax.set_title('time step='+str(i)) ax.set_xlabel('position (m)') ax.set_ylabel('position (m)') ax.scatter(allX[i][0],allX[i][1],c='r'); #grab temp images fig.savefig(os.path.dirname(__file__) + '/' + '../tmp/img'+str(i)+".png",bbox_inches='tight',pad_inches=0) plt.pause(.1) #Animate Results fig,ax=plt.subplots() images=[] for k in range(0,simSteps): fname=os.path.dirname(__file__) + '/' + '../tmp/img%d.png' %k img=mgimg.imread(fname) imgplot=plt.imshow(img) plt.axis('off') images.append([imgplot]) ani=animation.ArtistAnimation(fig,images,interval=20) if(not os.path.isdir(os.path.dirname(__file__) + '/' + '../results/'+self.problemName)): os.mkdir(os.path.dirname(__file__) + '/' + '../results/'+self.problemName); if(not greedySim): path = os.path.dirname(__file__) + '/' + '../results/'+self.problemName+'/' + self.problemName + '_Animation' + self.alphaNum + '.gif'; else: path=os.path.dirname(__file__) + '/' + '../results/'+self.problemName+'/' + self.problemName + '_Animation_Greedy' + self.alphaNum + '.gif'; ani.save(path,fps=2,writer='animation.writer')
def testMakeNear(): pzIn = Softmax() pzOut = Softmax() cent = [4, 4] orient = 0 nearness = 2 lengthIn = 3 lengthOut = lengthIn + nearness widthIn = 2 widthOut = widthIn + nearness pzIn.buildOrientedRecModel(cent, orient, lengthIn, widthIn, steepness=10) pzOut.buildOrientedRecModel(cent, orient, lengthOut, widthOut, steepness=10) #pzIn.plot2D(low=[0,0],high=[10,10]); #pzOut.plot2D(low=[0,0],high=[10,10]); b = GM() for i in range(0, 10): for j in range(0, 10): b.addG(Gaussian([i, j], [[1, 0], [0, 1]], 1)) b.normalizeWeights() b1 = GM() for i in range(1, 5): b1.addGM(pzIn.runVBND(b, i)) b1.normalizeWeights() b2 = GM() b2.addGM(pzOut.runVBND(b1, 0)) b2.normalizeWeights() fig, axarr = plt.subplots(3) [x, y, c] = b.plot2D(low=[0, 0], high=[10, 10], vis=False) axarr[0].contourf(x, y, c) [x, y, c] = b1.plot2D(low=[0, 0], high=[10, 10], vis=False) axarr[1].contourf(x, y, c) [x, y, c] = b2.plot2D(low=[0, 0], high=[10, 10], vis=False) axarr[2].contourf(x, y, c) plt.show()
def VL(pri, like, softClass, low, high, res): [xs, softmax] = like.plot1D(low, high, res, vis=False) ys = [0] * len(xs) for i in range(0, len(xs)): ys[i] = np.log(pri.pointEval(xs[i])) + np.log(softmax[softClass][i]) newMean = xs[ys.index(max(ys))] newVar = -(pri.getVars()[0])**(-1) secDiv = likeDiv(like, softClass, newMean) newVar = newVar - 1 / (softmax[softClass][ys.index( max(ys))])**2 * (secDiv * secDiv) newVar = -(newVar)**(-1) ans = GM() ans.addG(Gaussian(newMean, newVar, pri.getWeights()[0])) return ans
def getMDPAction(self, x): maxVal = -10000000 maxGM = GM() bestAct = 0 for a in range(0, len(self.delA)): suma = GM() for g in self.ValueFunc.Gs: mean = (np.matrix(g.mean) - np.matrix(self.delA[a])).tolist() var = (np.matrix(g.var) + np.matrix(self.delAVar)).tolist() suma.addG(Gaussian(mean, var, g.weight)) suma.addGM(self.r) tmpVal = suma.pointEval(x) if (tmpVal > maxVal): maxVal = tmpVal maxGM = suma bestAct = a return bestAct
def testMakeNear(): pzIn = Softmax(); pzOut = Softmax(); cent = [3.5,3.5]; orient = 0; nearness = 2; lengthIn = 3; lengthOut = lengthIn+nearness; widthIn = 2; widthOut = widthIn+nearness; pzIn.buildOrientedRecModel(cent,orient,lengthIn,widthIn,steepness=10); pzOut.buildOrientedRecModel(cent,orient,lengthOut,widthOut,steepness=10); #pzIn.plot2D(low=[0,0],high=[10,10]); #pzOut.plot2D(low=[0,0],high=[10,10]); b = GM(); for i in range(0,11): for j in range(0,11): b.addG(Gaussian([i,j],[[1,0],[0,1]],1)); b.normalizeWeights(); b1 = GM(); for i in range(1,5): b1.addGM(pzIn.runVBND(b,i)); b1.normalizeWeights(); b2 = GM(); b2.addGM(pzOut.runVBND(b1,0)); b2.normalizeWeights(); fig,axarr = plt.subplots(3); [x,y,c] = b.plot2D(low=[0,0],high=[10,10],vis=False); axarr[0].contourf(x,y,c); [x,y,c] = b1.plot2D(low=[0,0],high=[10,10],vis=False); axarr[1].contourf(x,y,c); [x,y,c] = b2.plot2D(low=[0,0],high=[10,10],vis=False); axarr[2].contourf(x,y,c); plt.show();
def transformPolToCart(bpol, offset=[0, 0]): bcart = GM() for g in bpol: m = g.mean mPrime = [ m[0] * math.cos(m[1]) + offset[0], m[0] * math.sin(m[1]) + offset[1] ] J11 = math.cos(m[1]) J12 = -m[0] * math.sin(m[1]) J21 = math.sin(m[1]) J22 = m[0] * math.cos(m[1]) JPolCar = np.matrix([[J11, J12], [J21, J22]]) var = np.matrix(g.var) varPrime = (JPolCar * var * JPolCar.T).tolist() bcart.addG(Gaussian(mPrime, varPrime, g.weight)) return bcart
def simulate(self, steps, initState=None): if (initState is None): initState = self.states[0] states = [] obs = [] states.append(initState) for step in range(0, steps): #get new state keys, vals = zip(*self.Tprob[states[-1]].items()) states.append(np.random.choice(keys, p=vals)) newGM = GM() newGM.addG(self.Oprob[self.states.index(states[-1])]) obs.append(newGM.sample(1)[0]) return states, obs
def testInvertedSoftmaxModels(): b = GM() b.addG(Gaussian([2, 2], [[1, 0], [0, 1]], 1)) b.addG(Gaussian([4, 2], [[1, 0], [0, 1]], 1)) b.addG(Gaussian([2, 4], [[1, 0], [0, 1]], 1)) b.addG(Gaussian([3, 3], [[1, 0], [0, 1]], 1)) b.normalizeWeights() b.plot2D() pz = Softmax() pz.buildOrientedRecModel([2, 2], 0, 1, 1, 5) #pz.plot2D(); startTime = time.clock() b2 = GM() for i in range(1, 5): b2.addGM(pz.runVBND(b, i)) print(time.clock() - startTime) b2.plot2D() startTime = time.clock() b3 = GM() b3.addGM(b) tmpB = pz.runVBND(b, 0) tmpB.normalizeWeights() tmpB.scalerMultiply(-1) b3.addGM(tmpB) tmpBWeights = b3.getWeights() mi = min(b3.getWeights()) #print(mi); for g in b3.Gs: g.weight = g.weight - mi b3.normalizeWeights() print(time.clock() - startTime) #b3.display(); b3.plot2D()
def separateAndNormalize(mix): #Takes a mixture and returns two mixtures #and two anti-normalizing constants posMix = GM() negMix = GM() posSum = 0 negSum = 0 for g in mix: if (g.weight < 0): negSum += g.weight negMix.addG(deepcopy(g)) else: posSum += g.weight posMix.addG(deepcopy(g)) posMix.normalizeWeights() negMix.normalizeWeights() return [posMix, negMix, posSum, negSum]
def testLWIS(): pz = Softmax() pose = [0, 0, 0] pz.buildTriView(pose, length=2, steepness=10) prior = GM() #prior.addG(Gaussian([1,0],[[1,0],[0,1]],1)); for i in range(0, 100): prior.addG( Gaussian([np.random.random() * 4 - 2, np.random.random() * 4 - 2], [[0.1, 0], [0, 0.1]], 1)) prior.normalizeWeights() post = GM() for g in prior: post.addG(pz.lwisUpdate(g, 0, 500, inverse=True)) #post.display(); [x1, y1, c1] = prior.plot2D(low=[-5, -5], high=[5, 5], vis=False) [x3, y3, c3] = pz.plot2D(low=[-5, -5], high=[5, 5], vis=False) [x2, y2, c2] = post.plot2D(low=[-5, -5], high=[5, 5], vis=False) diffs = c2 - c1 print(np.amax(c2)) print(np.amax(diffs)) print(np.amin(diffs)) fig, axarr = plt.subplots(4) axarr[0].contourf(x1, y1, c1) axarr[0].set_title('Prior') axarr[1].contourf(x3, y3, c3) axarr[1].set_title('Likelihood') axarr[2].contourf(x2, y2, c2) axarr[2].set_title('Posterior') axarr[3].contourf(x2, y2, diffs) axarr[3].set_title('Diffs') plt.show()