# Fit robot simulator to robot

I have odometry data $(x, y, angle)$ of a real two-wheeled robot, who received control commands $(forward speed, angular speed)$.

Now I want to code a motion model (in C++ (/ROS)) which should follow the same trajectory, given the same control commands.

Normally, the kinematics should look something like this:

\begin{align} v_{fwd} &= control_{fwd} \\ v_{ang} &= control_{ang} \\ x &= x_{old} + 0.5(control_{fwd} + v_{fwd,old}) * \cos(angle) * dt \\ y &= y_{old} + 0.5(control_{fwd} + v_{fwd,old}) * \sin(angle) * dt \\ angle &= angle_{old} + 0.5(control_{ang} + v_{ang,old}) * dt \end{align}

And I thought about just setting

\begin{align} v_{fwd} &= control_{fwd} + k_1 v_{fwd,old} + k_2 v_{fwd,old}^2 + k_3 v_{ang,old} + k_4 v_{ang,old}^2 \\ v_{ang} &= \text{ ...analog...} \\ x, y, angle &\text{ unchanged} \end{align}

and then just search the minimum of the squared distance of the computed trajetory to the real one - depending on the values of $k_i$. This would mean either a good optimization algorithm or just brute-forcing / randomly testing a lot of values.

Is this the way to go here? I tried the 2nd approach, but the results so far are not that good.

So, as you might guess now, I'm pretty new at this, so any help is appreciated.

• How great are the error values of wheel slip, and angle inaccuracy? The model could be perfect, but if your real-world errors are high, than the model will deviate strongly from reality, regardless of algorithm, unless this is taken into effect. – Mhz4.77 Apr 4 '14 at 1:41