SLAM-Course-Solutions

SLAM-Course-Solutions copied to clipboard

mu(1, 1) = mu(1) + (u.t * cos(mu(3) + u.r1));
mu(2, 1) = mu(2) + (u.t * sin(mu(3) + u.r1));
mu(3, 1) = mu(3) + (u.r1 + u.r2);
mu(3, 1) = normalize_angle(mu(3, 1));

% Compute the 3x3 Jacobian Gx of the motion model
Gx = eye(3, 3);
Gx(1, 3) = -u.t * cos(mu(3) + u.r1);
Gx(2, 3) = u.t * sin(mu(3) + u.r1);

Jacobian should be

Gx = eye(3, 3);
Gx(1, 3) = -u.t * sin(mu(3) + u.r1);
Gx(2, 3) = u.t * cos(mu(3) + u.r1);