Wheel Odometry Covariance Matrix for Custom Robot

Question

I have been looking around in the forums, but could not find any answer for how to go about getting the wheel odometry covariance matrix for a custom built planar robot (I found some posts related to EKF, but could not find any clear solution). I need this in order to for e.g. fuse wheel odometry with other types of odometry, etc... I really would only need the covariance for the global planar velocities (which are directly related to the encoders/inputs), and determining the position or acceleration covariances seem to be able to be derived from there.

I will try posting the answers to this as I find them, and perhaps this could also help future roboticists (hobbysts!?) that want to build a new mobile robot but may be confused about this.

Let's say I have a custom planar robot with N number of motors/wheels/encoders, and a defined kinematic model.

That is, I have a mapping:

(Vx, Vy, AngVel) -> ( W1, W2, ..., Wn)

where W's are each motor's angular velocities. I am not sure if the inverse mapping always exists, but I could assume that it does just for now.

By reading around the forums, I found that first we should calibrate for the systematic errors (e.g., due to unequal wheel diameters, etc..). For differential wheeled robots, this can be done with the UMBMark algorithm. This still does not give any specific information on how to get the covariance matrix tho.

I imagine there are two options, using a static covariance matrix (predetermined by calibration), or dynamically adjusting them (let's say through a Kalman filter).

A static covariance matrix is probably less accurate, but simpler to determine. However, I have no idea how to go about choosing these values (should I make the robot move back and forth several times and use the error as the Vx variance?). Are there any basic guidelines for filling up the covariance matrix statically?

Another (more difficult) option seems to be to use a Kalman filter, and update the covariance matrices dynamically. But I am unsure what to choose as the inputs, nor white gaussian noise values for process/observation.

Imagine there's some sort of local controller that I just give desired angular velocities and it tries to produce them. Should I go as low level as defining my inputs as the currents, and then go through the motor model? Or should I just choose the inputs to be my commanded angular velocities?

But if my inputs are the desired angular velocities, then the state doesn't seem to depend on the previous state, and doesn't follow the kalman filter convention for the process (i.e. the new state would only depend on the input, as I am controlling the planar velocities directly from the wheels, and it is not affected by the previous state)!? At least the sensor model seems to be quite easy to derive from the kinematic model.

As you can probably see, I am extremely confused for something that most likely has to be determined for most mobile robots out there. I am finding little to no clear documentation, which is weird for such a (very common!?) problem. If anyone could point me in the right direction I'd be extremely happy!

Thanks!!

Answer 1

The covariance matrix of the control inputs is measured and known.

That is, following the EKF equations on this page, the covariance of the control, $Q$ is (often) a diagonal matrix, where the diagonal terms are the variances of the control effects.

In a planar 2d robot, where odometery is used as a surrogate of control, the $Q$ matrix has two non-zero elements, which coorespond to the error in distance measurements from odometry. Stated another way, if the odometers return $d$ as distance travelled, and they have error $\sigma_d$, then your covariance matrix for control is $\begin{bmatrix} \sigma^2_d, 0 \\ 0, \sigma^2_d \end{bmatrix}$

That's it. Just estimate how bad your measurements are.

• I would not go all the way down to voltages, I would trust the local controller and measure how bad it is to get the sigma above
• If your robot does not have inertia, then yes, angular velocity is independent across states. This is not a KF problem, you could include it if you wanted
• You need a book, and to start simple. Start with odometry. Do a 3D (x,y, theta) state, and see if you can go from there.

---

The covariance matrix of the robot state is constructed by the jacobian of the state with respect to the control inputs, and is part of the EKF update equations. (or, for non-parametric estimators, it's often just the actual variance of the samples).

---

Some details and references:

if your state (position, velocity) is a function of the wheels speeds (controls), first construct the functions $x=f(u), y=f(u), s=f(u), \omega = f(u)$... (for x,y position, speed, and rotational speed). You can go as crazy as you want for $u$, it could be just wheel speeds, or some hugely complex modelling of the whole world, if mercury is in retrograde, etc.

Typically, I use this.

Then, construct the jacobian, and let's call it $F$, etc, etc, following the ekf equations.

Answer 2

Josh, thank you for your help!

From my understanding it seems that a basic process for determining the velocity covariance matrices of a 3D (X,Y, Theta) robot with N wheels would then be (please correct me if I am wrong!):

1. Define the state of the robot. For example, this could be (x Velocity $\dot{x}$, y velocity $\dot{y}$, theta velocity $\dot{\theta}$).

2. Define a mapping function $x=f_x(u), y=f_y(u), \theta=f_{\theta}(u)$ of the control inputs $u$ to the state variables. This would depend on the kinematic model of the robot. For example, the control inputs could be the wheel velocities $w_1, w_2, ..., w_n$ for $N$ wheels.

3. Place the robot at a reference position on the floor. Give control inputs to move it in the X direction (for e.g. 0.5 m/s), measure the real velocity the robot moved in X (e.g. 0.45 m/s). Repeat this several times for the forward direction, and obtain the standard deviation in x velocity based on statistical measurements. Do the same process for y velocity and Theta velocity. This constant covariance matrix we will call Q:

$$ Q = \begin{bmatrix} \sigma_{\dot{x}}^2 &0 &0 \\ 0& \sigma_{\dot{y}}^2&0 \\ 0& 0& \sigma_{\dot{\theta}^2} \end{bmatrix} $$

3. Compute the transition matrix $F$, which is constructed from the jacobian of the state function $f$ with respect to the states. Refer to Wikipedia for the matrix equations.

4. Define the sensor model. This maps the state variables to sensor values. For wheeled robots with $N$ encoders $p_1, p_2, ..., p_N$, this would be a map of the form $p_1 = h_1(\dot{x},\dot{y},\dot{\theta});p_2 = h_2(\dot{x},\dot{y},\dot{\theta});... $.

5. Define the sensor error covariance matrix $R$. A wheel can be turned to a specific value, and the real measurements compared with the encoder outputs. When done several times, its statistics can be determined (similar to above):

$$ R = \begin{bmatrix} \sigma_{p_1}^2 &0 &0 \\ 0& \sigma_{p_2}^2&0 \\ 0& 0& ... \end{bmatrix} $$

6. Compute the matrix $H$, as the jacobian of the sensor model function $h$ to the state variables.

7. Now you can start the kalman filter loop. Choose matrix $P_0$ as an initial guess (you can choose all 0 for the beginning). Follow the equations in Wikipedia and solve for the state and odometry covariance matrix $P$ at every frame! This result (state, covariance) can be read by for example, the robot_localization package in ROS to fuse several sets of data and get a better odometry approximation!

P.S. I am not really sure if this is correct, but I hope someone can tell me if it is (or any mistakes that I may have done along the way!)!