3

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?

Greenonline
  • 1,508
  • 4
  • 18
  • 32
half-potato
  • 141
  • 5

3 Answers3

5

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).

charles
  • 341
  • 1
  • 7
0

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.

Falko
  • 121
  • 7
0

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]
CroCo
  • 2,453
  • 1
  • 17
  • 40