Advertisement
Guest User

Untitled

a guest
Mar 29th, 2015
146
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 0.57 KB | None | 0 0
  1.     def predict(self, deltaPose):
  2.         self.stateVector[0] += deltaPose.x
  3.         self.stateVector[1] += deltaPose.y
  4.         self.stateVector[2] += deltaPose.r
  5.  
  6.         self.jacobianA[0][2] = -deltaPose.y
  7.         self.jacobianA[1][2] = deltaPose.x
  8.  
  9.         self.jacobianJXR[0][2] = -deltaPose.y
  10.         self.jacobianJXR[1][2] = deltaPose.x
  11.  
  12.         self.jacobianJZ[0][0] = math.cos(self.stateVector[2])
  13.         self.jacobianJZ[0][1] = -self.stateVector[1]
  14.         self.jacobianJZ[1][0] = math.sin(self.stateVector[2])
  15.         self.jacobianJZ[1][1] = self.stateVector[0]
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement