## Navigation update equations for a two wheeled robot

These equations assume that the robot is traveling in a circular path with constant turn rate. This is typical of a robot that has set its motor speeds constant but different for left and right speeds. The nonlinear form involves sin and cos of both the present and last heading and involves division by the change in heading. It is more accurate but can lead to division by zero if the robot is traveling in a straight line.. i. e. it has infinite turn radius. The approximate form which is valid for small heading changes between updates is more robust since it doesn’ t require division by the heading change.

If a gyro is available with heading output then you don’t need to compute heading, otherwise, heading is derived from the difference in encoders.   Usually, the navigation equations are run periodically at around 20 to 50 hz.    Rates are derived by dividing the delta changes in position and heading by the update time.  Velocities can be noisy when the update times are short so often an exponential filter is used to smooth the outputs before use in control laws.

In the figure, psi_1 and psi_2 represent last and present heading values respectively.   These are defined positive counter clockwise.   If your coordinate system is N, E and  positive heading clockwise  from N then you would modify the equations by letting delta E = delta x , delta N = delta y and replace psi_ccw with 90 -psi_cw since we are now defining psi_cw positive to the right of the y axis instead of the x axis.  So  the NE coordinates would look like this:

N = N + (sl + sr)/2 *(cos(psi_cw) – delta_psi*(sin(psi_cw))

E = E  + (sl + sr)/2*(sin(psi_cw)   + delta_psi*(cos(psi_cw))