3
$\begingroup$

I am reading SLAM for Dummies, which you can find on Google, or at this link: SLAM for Dummies - A Tutorial Approach to Simultaneous Localization and Mapping.

They do some differentiation of matrices on page 33 and I am getting different answers for the resulting Jacobian matrices.

The paper derived

$$ \left[ {\begin{array}{c} \sqrt{(\lambda_x - x)^2 + (\lambda_y - y)^2} + v_r \\ \tan^{-1}\left(\frac{\lambda_y - y}{\lambda_y - x}\right) - \theta + v_\theta \end{array}} \right] $$

and got $$ \left[ {\begin{array}{ccc} \frac{x - \lambda_y}{r},& \frac{y - \lambda_y}{r},& 0\\ \frac{\lambda_y - y}{r^2},& \frac{\lambda_y - x}{r^2},& -1 \end{array}} \right] $$

I don't get where the $r$ came from. I got completely different answers. Does anybody know what the $r$ stands for? If not, is there a different way to represent the Jacobian of this matrix?

$\endgroup$
5
$\begingroup$

From the paper:

$\begin{bmatrix} range\\bearing \end{bmatrix} = \begin{bmatrix} \sqrt{(\lambda_x-x)^2 + (\lambda_y - y)^2 } + v_r \\ tan^{-1}(\frac{\lambda_y-y}{\lambda_x-x}) - \theta + v_{\theta} \end{bmatrix}$

The Jacobian is:

$H = \begin{bmatrix}\frac{\partial range}{\partial x} & \frac{\partial range}{\partial y} & \frac{\partial range}{\partial \theta}\\\frac{\partial bearing}{\partial x} & \frac{\partial bearing}{\partial y} & \frac{\partial bearing}{\partial \theta}\end{bmatrix} = \begin{bmatrix}\frac{-2(\lambda_x-x)}{2\sqrt{(\lambda_x-x)^2 + (\lambda_y - y)^2 }} & \frac{-2(\lambda_y-y)}{2\sqrt{(\lambda_x-x)^2 + (\lambda_y - y)^2 }}&0\\ \frac{1}{1+(\frac{\lambda_y-y}{\lambda_x-x})^2}\cdot\frac{\lambda_y-y}{(\lambda_x-x)^2}&\frac{1}{1+(\frac{\lambda_y-y}{\lambda_x-x})^2}\cdot\frac{-1}{(\lambda_x-x)}&-1\end{bmatrix}$

The expression $\sqrt{(\lambda_x-x)^2 + (\lambda_y - y)^2 }$ is just the ideal range (distance from the robot to the landmark), and so is called $r$. Then the previous expression simplifies to:

$H = \begin{bmatrix}\frac{x-\lambda_x}{\sqrt{(\lambda_x-x)^2 + (\lambda_y - y)^2 }} & \frac{y-\lambda_y}{\sqrt{(\lambda_x-x)^2 + (\lambda_y - y)^2 }}&0\\ \frac{\lambda_y-y}{(\lambda_x-x)^2+(\lambda_y-y)^2}&\frac{x-\lambda_x}{(\lambda_x-x)^2+(\lambda_y-y)^2}&-1\end{bmatrix} = \begin{bmatrix}\frac{x-\lambda_x}{r} & \frac{y-\lambda_y}{r}&0\\ \frac{\lambda_y-y}{r^2}&\frac{x-\lambda_x}{r^2}&-1\end{bmatrix}$

As you can see there is a sign error in the paper (second row, second column). You can check it with any differentiation tool (you can find a bunch of them online).

$\endgroup$
0
$\begingroup$

Without checking, I assume $r$ is introduced as the range $\sqrt{(\lambda_x - x)^2 + (\lambda_y - y)^2}$. This is often useful when deriving such expressions, since the range often occurs in the derivative in first or higher order.

$\endgroup$
0
$\begingroup$

Besides the answers posted above, you can use Matlab to acquire H.

syms my mx x y r p a
r = sqrt( (my-y)^2 + (mx-x)^2 ); % range equation (r)
p = atan( (my-y)/(mx-x) ) - a;   % bearing equation (\phi)
 input = [x y a];
output = [r p];
H = jacobian(output, input)

The result is

H =
[ -(2*mx - 2*x)/(2*((mx - x)^2 + (my - y)^2)^(1/2)), -(2*my - 2*y)/(2*((mx - x)^2 + (my - y)^2)^(1/2)),  0]
[ (my - y)/((mx - x)^2*((my - y)^2/(mx - x)^2 + 1)),         -1/((mx - x)*((my - y)^2/(mx - x)^2 + 1)), -1]
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.