Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #ADDING LANDMARK
- for landmark in newLandmarks:
- #This simply creates a new row and column filled with zeros, to the covariance matrix.
- self.addLandmark(landmark)
- la = math.radians(landmark.angle)
- #This is the 2x3 jacobian of the landmark with respect to range and bearing. This as 1's along diag.
- #Here the local landmark angle gets added to the angle of the robot, stored in the state vector X.
- self.JXR[0][2] = -landmark.dist * math.sin(la + self.X[2])
- self.JXR[1][2] = landmark.dist * math.cos(la + self.X[2])
- #This is the 2x2 jacobian of the landmark with respect to the robot's state X.
- self.JZ[0][0] = math.cos(la + self.X[2])
- self.JZ[0][1] = -landmark.dist * math.sin(la + self.X[2])
- self.JZ[1][0] = math.sin(la + self.X[2])
- self.JZ[1][1] = landmark.dist * math.cos(la + self.X[2])
- #This is the 2x2 noise matrix, 'sensorAngleSigma' is equivalent to 1 degree, in radians.
- R = np.array([[landmark.dist*self.sensorDistSigma, 0],[0, self.sensorAngleSigma]])
- #This grabs (and also sets) the 2x2 covariance of the new landmark.
- #The multiples the jacobian JXR with the robot's 3x3 covariance, with JXR transposed.
- #Then the result of the noise matrix times the jacobian JZ is added.
- self.getLM(landmark.id)[:] = matmult(self.JXR, self.getRR(), self.JXR.T) + matmult(self.JZ, R, self.JZ.T)
- #Here the new bottom row is being set, as the cross-variance between the jacobian JXR and the very top row.
- #The robot-landmark cross-variances.
- self.P[3+(self.landmarkCount*2)-2:, 0:-2] = matmult(self.JXR, self.P[0:3, 0:3+(self.landmarkCount*2)-2])
- #Here the new column is set, same as above but the transposed version.
- self.P[0:-2, 3+(self.landmarkCount*2)-2:] = matmult(self.JXR, self.P[0:3, 0:3+(self.landmarkCount*2)-2]).T
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement