2
$\begingroup$

Say, we are dealing with an odometric localization problem.

In the below example, $(x_k, y_k), (x_l, y_l)$ are cartesian coordinates of the sensor and the landmark respectively. Then the output equation is:

$z_k = atan2(y_k − y_l, x_k − x_l)$

Is the following the correct way of linearizing this equation?

$H_{k+1} = (\frac{−(\hat{y}_{k+1|k} − y_l)}{(\hat{x}_{k+1|k} − x_l)^2 + (\hat{y}_{k+1|k} − y_l)^2} \frac{(\hat{x}_{k+1|k} − x_l)}{(\hat{x}_{k+1|k} − x_l)^2 + (\hat{y}_{k+1|k} − y_l)^2})$

Can you provide other resources and examples to practice?

Improve this question
$\endgroup$
5
  • $\begingroup$ @Chuck, thank you a ton for your corrections, please forgive my lack of detailing, I am new here and I will try to be more careful in the future. Meanwhile, I edited my questions, is the format better now? $\endgroup$
    – Leon Rai
    Commented Feb 11, 2019 at 14:00
  • 1
    $\begingroup$ @LeonRai - Looks better, but please try to show how you would approach the problem and why you think you're doing it incorrectly. Your prior edit made it seem like linearizing the process equations was not a problem, so what makes these equations different? Even a little bit of information here is useful, like, "I don't know how to linearize inverse trig functions," or, "I linearized the arctangent, but I get X when I expected to get Y for an input of <inputs>." Hopefully you can see how these two different statements can lead to very different answers; they cover different problems entirely. $\endgroup$
    – Chuck ♦
    Commented Feb 11, 2019 at 14:05
  • $\begingroup$ I feel like it is not right to directly ask about your problem but rather ask a general question which others can benefit too, therefore I tend to ask a more theoretical question. I see now, the more specific the problem, the better people can help. Thank you again. Hope my edits will make more sense now. $\endgroup$
    – Leon Rai
    Commented Feb 11, 2019 at 14:28
  • $\begingroup$ It's still a little vague; you jump from a $z = ... $ straight to $H = ...$ with no explanation of how you got there. If you're asking if it's the correct way then you probably suspect it's not the correct answer. If that's the case then why do you think it's not right? How did you get from $z_k$ to $H_{k+1}$? $z_k$ is a singular value, but you have a comma in the $H_{k+1}$ expression; did you mean for that to be an arctangent, too, or is the comma a typo? As I wrote earlier, it's hard to point out where you're going wrong if you don't step us through your solution. $\endgroup$
    – Chuck ♦
    Commented Feb 11, 2019 at 14:44
  • $\begingroup$ Well, it was a typo then. As I said I don't know how to go from $z_k$ to $H_{k+1}$, I found some articles and that brought me this far. I am not sure how to explain what I don't know how to explain. Thanks. $\endgroup$
    – Leon Rai
    Commented Feb 11, 2019 at 14:53

1 Answer 1

Reset to default
2
$\begingroup$

Hi and welcome to stack exchange: robotics edition.

Yes. Your derivation for the landmark update Jacobian is correct. If you are doing SLAM with respect to the landmarks, don't forget to form the Jacobian with respect to all state variables, including the Robot's $x$ and $y$ points.

What I mean is, if the state of the system is $x_r,y_r,x_t,y_t$, and the measurement equation is as shown, then the Jacbian of the measurement equation $H$ is $[\frac{\partial h}{\partial x_r},\frac{\partial h}{\partial y_r}, \frac{\partial h}{\partial x_t},\frac{\partial h}{\partial y_t}]$

But ... (spoiler alert)

surprise, surprise, \frac{\partial h}{\partial x_r},\frac{\partial h}{\partial y_r} is the same as those two entries with a sign flip as you'll see.

Some complaints about the question center around the notation and steps. You are very clearly asking if you have formed the correct Jacobian matrix for the bearing measurement equation. But, that's not clear to everyone, so you may want to write a more thorough background of what you're trying to do, and what notation you're using. (If for no other reason than Google will pick it up better).

If you want to practice forming Jacobian matrices, I suggest you choose $n$ functions of $m$ variables, and practice taking partial derivatives. Use Wolfram Alpha to check the partials, and use the structure of a Jacobian from Wikipedia to check the composition of the matrix. Robotics-specific examples are limited to $h$=range, bearing, or position estimates. So, $h=||r-l||$, the $atan2$ example from above, and $h=(x_l,y_l)$ are the best you can do. You can always form the measurement function $h$ as being to two or more landmarks simultaneously, as well. This is not a good discussion for Robotics Stack Exchange, however.

Improve this answer
$\endgroup$
3
  • $\begingroup$ Thank you very much for the answer. However, I am not sure if I understand the part "don't forget to form the Jacobian with respect to all state variables". Where can I find some examples to practice? $\endgroup$
    – Leon Rai
    Commented Feb 11, 2019 at 17:35
  • 1
    $\begingroup$ I've udpated the answer to be more in line with your request, however, providing practice sets probably isn't the best use of this forum. I suggest a textbook or two, and inventing your own measurement functions which take in robot and landmark positions. Forming a funciton $h$ which is a non-linear combination of the robot and many different landmark positions would be plenty of challenge. $\endgroup$
    – Josh Vander Hook
    Commented Feb 11, 2019 at 17:42
  • $\begingroup$ Amazing! Thank you for being very detailed. I got it now. $\endgroup$
    – Leon Rai
    Commented Feb 11, 2019 at 18:13

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

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