Hi,
I've recently been implementing an extended kalman filter (EKF-SLAM) that makes heavy use of jacobians. But I don't believe I'm calculating them correctly. (Apologies for the lack of proper notation, I am very much a math newb.)
There are two jacobians I'm stuck on. They should be with respect to my 'robot's' X, Y and rotation, and a separate 'landmark' X and Y location.
The landmark element is transformed from polar co-ordinates to cartesian using this function:
landmark.x = landmark.range * cos(landmark.angle)
landmark.y = landmark.range * -sin(landmark.angle)
(Note I am using negative sin here, which is different from all the examples I've found. Does this impact the jacobian?)
The 'landmark' is then transformed to 'world' space, given the state variables of the robot, using this function:
landmark.x = (landmark.x * cos(robot.rotation) - landmark.y * -sin(robot.rotation)) + robot.x
landmark.y = (landmark.y * -sin(robot.rotation) + landmark.y * cos(robot.rotation)) - robot.y
My assumption of the jacobian with respect to the landmark:
cos(landmark.angle + robot.angle) | landmark.range * sin(landmark.angle + robot.angle)
-sin(landmark.angle + robot.angle) | landmark.range * cos(landmark.angle + robot.angle)
Is this correct? And what should the jacobian with respect to the robot state look like?
I believe the examples I've tried don't work because my transform and conversion functions vary slightly.
The jacobians are described at the bottom of page 47 (actual page 35): http://www.nada.kth.se/utbildning/forsk.utb/avhandlingar/lic/020220.pdf
Thanks in advance!
I've recently been implementing an extended kalman filter (EKF-SLAM) that makes heavy use of jacobians. But I don't believe I'm calculating them correctly. (Apologies for the lack of proper notation, I am very much a math newb.)
There are two jacobians I'm stuck on. They should be with respect to my 'robot's' X, Y and rotation, and a separate 'landmark' X and Y location.
The landmark element is transformed from polar co-ordinates to cartesian using this function:
landmark.x = landmark.range * cos(landmark.angle)
landmark.y = landmark.range * -sin(landmark.angle)
(Note I am using negative sin here, which is different from all the examples I've found. Does this impact the jacobian?)
The 'landmark' is then transformed to 'world' space, given the state variables of the robot, using this function:
landmark.x = (landmark.x * cos(robot.rotation) - landmark.y * -sin(robot.rotation)) + robot.x
landmark.y = (landmark.y * -sin(robot.rotation) + landmark.y * cos(robot.rotation)) - robot.y
My assumption of the jacobian with respect to the landmark:
cos(landmark.angle + robot.angle) | landmark.range * sin(landmark.angle + robot.angle)
-sin(landmark.angle + robot.angle) | landmark.range * cos(landmark.angle + robot.angle)
Is this correct? And what should the jacobian with respect to the robot state look like?
I believe the examples I've tried don't work because my transform and conversion functions vary slightly.
The jacobians are described at the bottom of page 47 (actual page 35): http://www.nada.kth.se/utbildning/forsk.utb/avhandlingar/lic/020220.pdf
Thanks in advance!
Last edited: