Good day,

I have a robot with an IMU that tells Yaw Rate, and Magnetic heading. It also tells Xvelocity and YVelocity at that instance of the vehicle, on the vehicle frame. (So irrespective of heading, if the robot moved forward, yvelocity would change for example)

Assuming my robot starts at position (0,0) and Heading based on the Magnetic heading, I need to calculate the next position of the robot based on some world frame. How can I do this?


Use the basic formula's:

$x_t = x_0+v_x*t+\frac{1}{2}*a_x*t^2$

$y_t = y_0+v_y*t+\frac{1}{2}*a_y*t^2$

This way you can calculate all your $x$ and $y$ positions as long as your time steps are small enough so $v$ and $a$ don't change too much during each time step.


Since the magnetic heading ($\theta$) is known from the sensor, the values for $v_x$ and $v_y$ can be calculated:

$v_x = v_1*cos(\theta) +v_2*sin(\theta)$

$v_y = v_2*cos(\theta) -v_1*sin(\theta)$

In which $v_1$ is x-direction of your robot frame, $v_2$ is y-direction of your robot frame. $v_x$ is x-direction in world frame, $v_y$ is y-direction in world frame.

Edit 2:

If your sensor outputs angular velocity instead of magnetic heading, you can calculate the heading like it is an angular position, so in the same way you would calculate position:

$\theta_t = \theta_0 + \dot{\theta}*t+\frac{1}{2}*\ddot{\theta}*t^2$

Here $\theta_0$ is your heading at the start (this should be known). $\dot{\theta}$ is your angular velocity and $\ddot{\theta}$ is your angular acceleration.

  • $\begingroup$ I need to incorporate the angular rotation $\endgroup$
    – raaj
    Dec 28 '15 at 7:40
  • $\begingroup$ ok, thats a good start, but..one small thing. There are instances where my sensor doesnt output magnetic heading, but it outputs the angular velocity instead. how can i incorporate that into my equation? $\endgroup$
    – raaj
    Dec 28 '15 at 8:21
  • 1
    $\begingroup$ Shouldn't the calculation of $v_y$ have its $v_1$ component multiplied by negative $sin(\theta)$? $\endgroup$
    – Shahbaz
    Jan 5 '16 at 15:49
  • $\begingroup$ You're right, changed the formula. Also, thanks for the formatting! $\endgroup$
    – DrDonut
    Jan 6 '16 at 7:13
  • $\begingroup$ @DrDonut, sure! Take a minute to learn the basics of tex math though, it's very useful. Also, no big deal, but I liked the previous order of expressions for $v_y$, because you could more easily see the underlying rotation-matrix-multiplication. I guess I'm writing this more for future visitors to understand where those formulas came from. $\endgroup$
    – Shahbaz
    Jan 6 '16 at 14:29

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.