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!!