def beliefUpdate(modes,delA,delAVar,pz,bels,a,o,cond = -1): #Initialize btmp = GM(); for d in bels.Gs: for h in modes: for f in h.Gs: for l in pz[o].Gs: C1 = 1/(1/f.var + 1/d.var); c1 = C1*((1/f.var)*f.mean + (1/d.var)*d.mean); C2 = C1 + delAVar; c2 = c1+delA[modes.index(h)][a]; weight = d.weight*f.weight*l.weight*mvn.pdf(l.mean,c2,l.var+C2); var = 1/((1/l.var)+(1/C2)); mean = var*((1/l.var)*l.mean + (1/C2)*c2); g = Gaussian(mean,var,weight); btmp.addG(g); btmp.normalizeWeights(); if(cond != -1): btmp = btmp.kmeansCondensationN(k=cond,lowInit = -20,highInit=20); #btmp.condense(cond); for g in btmp: while(isinstance(g.var,list)): g.var = g.var[0]; btmp.display(); return btmp;
def beliefUpdate(self, belief, responses=None, copPoses=None): # #Create Cop View Cone # pose = copPoses[-1]; # viewCone = Softmax(); # viewCone.buildTriView(pose,length=1,steepness=10); # for i in range(0,len(viewCone.weights)): # viewCone.weights[i] = [0,0,viewCone.weights[i][0],viewCone.weights[i][1]]; #Update Cop View Cone # newerBelief = GM(); # for j in range(1,5): # tmpBel = viewCone.runVBND(belief,j); # if(j==1): # tmpBel.scalerMultiply(.4); # newerBelief.addGM(tmpBel); #Dont Update Cop View Cone #newerBelief = belief #4. update cops position to current position for g in belief: g.mean = [copPoses[-1][0], copPoses[-1][1], g.mean[2], g.mean[3]] g.var[0][0] = 0.1 g.var[0][1] = 0 g.var[1][0] = 0 g.var[1][1] = 0.1 #5. update belief with robber dynamics for g in belief: g.var[2][2] += 0.03 g.var[3][3] += 0.03 #Distance Cutoff #How many standard deviations away from the cop should gaussians be updated with view cone? distCut = 2 #Update Cop View Cone Using LWIS newerBelief = GM() for pose in copPoses: #Create Cop View Cone #pose = copPoses[-1]; viewCone = Softmax() viewCone.buildTriView(pose, length=1, steepness=10) for i in range(0, len(viewCone.weights)): viewCone.weights[i] = [ 0, 0, viewCone.weights[i][0], viewCone.weights[i][1] ] for g in belief: #If the gaussian is suffciently close to the pose #based on mahalanobis distance. #Logic: M-dist basically says how many standard devs away the point is from the mean #If it's more than distCut, it should be left alone gprime = Gaussian() gprime.mean = [g.mean[2], g.mean[3]] gprime.var = [[g.var[2][2], g.var[2][3]], [g.var[3][2], g.var[3][3]]] gprime.weight = g.weight #print(gprime.mean,gprime.mahalanobisDistance([pose[0]-np.cos(pose[2])*.5,pose[1]-np.sin(pose[2])*.5])); if (gprime.mahalanobisDistance([ pose[0] - np.cos(pose[2]) * .5, pose[1] - np.sin(pose[2]) * .5 ]) <= distCut): newG = viewCone.lwisUpdate(g, 0, 500, inverse=True) newerBelief.addG(newG) else: newerBelief.addG(g) #after each bayes update for the view cone, re-normalize #Just to be sure, it never hurts to check newerBelief.normalizeWeights() # newerBelief= belief #Update From Responses if (responses is not None): for res in responses: roomNum = res[0] mod = res[1] clas = res[2] sign = res[3] if (roomNum == 0): #apply to all if (sign == True): newerBelief = mod.runVBND(newerBelief, 0) else: tmp = GM() for j in range(1, mod.size): tmp.addGM(mod.runVBND(newerBelief, j)) newerBelief = tmp else: print('ROOM NUM: {}'.format(roomNum)) #apply to all rooms if (sign == True): newerBelief = mod.runVBND(newerBelief, clas) else: tmp = GM() for j in range(1, mod.size): if (j != clas): tmp.addGM(mod.runVBND(newerBelief, j)) newerBelief = tmp #Each response recieves a full bayes update, so we need to normalize each time newerBelief.normalizeWeights() #Condense the belief newerBelief.condense(15) print("*********************") print(newerBelief.size) print("*********************") #Make sure there is a belief in each room #A bit of a hack, but if this isn't here the lower level query fails # for room in self.map_.rooms: # 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(); # newerBelief.addG(Gaussian([0,0,centx,centy],var,0.00001)); #3. recombine beliefs (if we're still doing that sort of thing) newBelief = newerBelief newBelief.normalizeWeights() #Moved to before observation updates # #4. update cops position to current position # for g in newBelief: # g.mean = [copPoses[-1][0],copPoses[-1][1],g.mean[2],g.mean[3]]; # g.var[0][0] = 0.1; # g.var[0][1] = 0; # g.var[1][0] = 0; # g.var[1][1] = 0.1; # #5. update belief with robber dynamics # for g in newBelief: # g.var[2][2] += 0.05; # g.var[3][3] += 0.05; print("*********************") print(newBelief.size) print("*********************") return newBelief