# Untitled

a guest
Apr 6th, 2015
568
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
2.         for landmark in newLandmarks:
3.         #This simply creates a new row and column filled with zeros, to the covariance matrix.
5.
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