Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- def predict(self, deltaPose):
- self.stateVector[0] += deltaPose.x
- self.stateVector[1] += deltaPose.y
- self.stateVector[2] += deltaPose.r
- self.jacobianA[0][2] = -deltaPose.y
- self.jacobianA[1][2] = deltaPose.x
- self.jacobianJXR[0][2] = -deltaPose.y
- self.jacobianJXR[1][2] = deltaPose.x
- self.jacobianJZ[0][0] = math.cos(self.stateVector[2])
- self.jacobianJZ[0][1] = -self.stateVector[1]
- self.jacobianJZ[1][0] = math.sin(self.stateVector[2])
- self.jacobianJZ[1][1] = self.stateVector[0]
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement