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