Guest User

Untitled

a guest
Apr 6th, 2015
357
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #ADDING LANDMARK
  2.         for landmark in newLandmarks:
  3.         #This simply creates a new row and column filled with zeros, to the covariance matrix.
  4.             self.addLandmark(landmark)
  5.  
  6.             la = math.radians(landmark.angle)
  7.        
  8.         #This is the 2x3 jacobian of the landmark with respect to range and bearing. This as 1's along diag.
  9.         #Here the local landmark angle gets added to the angle of the robot, stored in the state vector X.
  10.             self.JXR[0][2] = -landmark.dist * math.sin(la + self.X[2])
  11.             self.JXR[1][2] = landmark.dist * math.cos(la + self.X[2])
  12.    
  13.         #This is the 2x2 jacobian of the landmark with respect to the robot's state X.
  14.             self.JZ[0][0] = math.cos(la + self.X[2])
  15.             self.JZ[0][1] = -landmark.dist * math.sin(la + self.X[2])
  16.             self.JZ[1][0] = math.sin(la + self.X[2])
  17.             self.JZ[1][1] = landmark.dist * math.cos(la + self.X[2])
  18.  
  19.             #This is the 2x2 noise matrix, 'sensorAngleSigma' is equivalent to 1 degree, in radians.
  20.             R = np.array([[landmark.dist*self.sensorDistSigma, 0],[0, self.sensorAngleSigma]])
  21.    
  22.         #This grabs (and also sets) the 2x2 covariance of the new landmark.
  23.         #The multiples the jacobian JXR with the robot's 3x3 covariance, with JXR transposed.
  24.         #Then the result of the noise matrix times the jacobian JZ is added.
  25.             self.getLM(landmark.id)[:] = matmult(self.JXR, self.getRR(), self.JXR.T) + matmult(self.JZ, R, self.JZ.T)
  26.    
  27.         #Here the new bottom row is being set, as the cross-variance between the jacobian JXR and the very top row.
  28.         #The robot-landmark cross-variances.
  29.             self.P[3+(self.landmarkCount*2)-2:, 0:-2] = matmult(self.JXR, self.P[0:3, 0:3+(self.landmarkCount*2)-2])
  30.  
  31.         #Here the new column is set, same as above but the transposed version.
  32.             self.P[0:-2, 3+(self.landmarkCount*2)-2:] = matmult(self.JXR, self.P[0:3, 0:3+(self.landmarkCount*2)-2]).T
RAW Paste Data