def buildReward(self, gen=True): if (gen): self.r = GM() var = [[1, 0, .7, 0], [0, 1, 0, .7], [.7, 0, 1, 0], [0, .7, 0, 1]] for i in range(-2, 8): for j in range(-2, 8): self.r.addG(Gaussian([i, j, i, j], var, 5.6)) for i in range(-2, 8): for j in range(-2, 8): for k in range(-2, 8): for l in range(-2, 8): if (abs(i - j) >= 2 or abs(k - l) >= 2): self.r.addG(Gaussian([i, j, k, l], var, -1)) print('Plotting Reward Model') self.plotAllSlices(self.r, title='Uncondensed Reward') print('Condensing Reward Model') self.r.condense(50) print('Plotting Condensed Reward Model') self.plotAllSlices(self.r, title='Condensed Reward') #f = open("../models/rewardModel4DIntercept.npy","w"); #np.save(f,self.r); file = 'models/rewardModel4DIntercept' self.r.printGMArrayToFile([self.r], file) else: #self.r = np.load("../models/rewardModel4DIntercept.npy").tolist(); file = 'models/rewardModel4DIntercept' tmp = GM() self.r = tmp.readGMArray4D(file)[0]
def buildAltObs(self, gen=True): #A front back left right center model #0:center #1-4: left,right,down,up if (gen): self.pz = [0] * 5 for i in range(0, 5): self.pz[i] = GM() var = [[.7, 0, 0, 0], [0, .7, 0, 0], [0, 0, .7, 0], [0, 0, 0, .7]] for i in range(-1, 7): for j in range(-1, 7): self.pz[0].addG(Gaussian([i, j, i, j], var, 1)) for i in range(-1, 7): for j in range(-1, 7): for k in range(-1, 7): for l in range(-1, 7): if (i - k > 0): self.pz[1].addG(Gaussian([i, j, k, l], var, 1)) if (i - k < 0): self.pz[2].addG(Gaussian([i, j, k, l], var, 1)) if (j - l > 0): self.pz[3].addG(Gaussian([i, j, k, l], var, 1)) if (j - l < 0): self.pz[4].addG(Gaussian([i, j, k, l], var, 1)) print('Plotting Observation Models') for i in range(0, len(self.pz)): self.plotAllSlices(self.pz[i], title='Uncondensed Observation') print('Condensing Observation Models') for i in range(0, len(self.pz)): self.pz[i] = self.pz[i].kmeansCondensationN( 50, lowInit=[-1, -1, -1, -1], highInit=[7, 7, 7, 7]) print('Plotting Condensed Observation Models') for i in range(0, len(self.pz)): self.plotAllSlices(self.pz[i], title='Condensed Observation') #f = open("../models/obsModel4DIntercept.npy","w"); #np.save(f,self.pz); file = 'models/obsAltModel4DIntercept' self.pz[0].printGMArrayToFile(self.pz, file) else: file = 'models/obsModel4DIntercept' tmp = GM() self.pz = tmp.readGMArray4D(file)
def buildObs(self, gen=True): if (gen): self.pz = [GM(), GM()] var = [[1, 0, .7, 0], [0, 1, 0, .7], [.7, 0, 1, 0], [0, .7, 0, 1]] for i in range(-2, 8): for j in range(-2, 8): self.pz[0].addG(Gaussian([i, j, i, j], var, 1)) for i in range(-2, 8): for j in range(-2, 8): for k in range(-2, 8): for l in range(-2, 8): if (abs(i - k) >= 2 or abs(j - l) >= 2): self.pz[1].addG(Gaussian([i, j, k, l], var, 1)) print('Plotting Observation Models') self.plotAllSlices(self.pz[0], title='Uncondensed Detection') self.plotAllSlices(self.pz[1], title='Uncondensed Non-Detect') print('Condensing Observation Models') self.pz[0].condense(20) self.pz[1] = self.pz[1].kmeansCondensationN( 45, lowInit=[-1, -1, -1, -1], highInit=[7, 7, 7, 7]) print('Plotting Condensed Observation Models') self.plotAllSlices(self.pz[0], title='Condensed Detection') self.plotAllSlices(self.pz[1], title='Condensed Non-Detect') #f = open("../models/obsModel4DIntercept.npy","w"); #np.save(f,self.pz); file = '../models/obsModel4DIntercept' self.pz[0].printGMArrayToFile(self.pz, file) else: file = '../models/obsModel4DIntercept' tmp = GM() self.pz = tmp.readGMArray4D(file)
def MDPValueIteration(self, gen=True): if (gen): #Intialize Value function self.ValueFunc = copy.deepcopy(self.r) for g in self.ValueFunc.Gs: g.weight = -1000 comparision = GM() comparision.addG( Gaussian( [1, 0, 0, 0], [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], 1)) uniform = GM() for i in range(0, 5): for j in range(0, 5): for k in range(0, 5): for l in range(0, 5): uniform.addG( Gaussian([i, j, k, l], [[4, 0, 0, 0], [0, 4, 0, 0], [0, 0, 4, 0], [0, 0, 0, 4]], 1)) count = 0 #until convergence while (not self.ValueFunc.comp(comparision) and count < 30): print(count) comparision = copy.deepcopy(self.ValueFunc) count += 1 #print(count); maxVal = -10000000 maxGM = GM() for a in range(0, 2): 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 = self.continuousDot(uniform, suma) if (tmpVal > maxVal): maxVal = tmpVal maxGM = copy.deepcopy(suma) maxGM.scalerMultiply(self.discount) maxGM = maxGM.kmeansCondensationN(20) self.ValueFunc = copy.deepcopy(maxGM) #self.ValueFunc.display(); #self.ValueFunc.plot2D(); print("MDP Value Iteration Complete") #f = open("../policies/MDP4DIntercept.npy","w"); #np.save(f,self.ValueFunc); file = "policies/MDP4DIntercept" self.ValueFunc.printGMArrayToFile([self.ValueFunc], file) else: #self.ValueFunc = np.load("../policies/MDP4DIntercept.npy").tolist(); file = "policies/MDP4DIntercept" tmp = GM() self.ValueFunc = tmp.readGMArray4D(file)[0]