Kalman filter Issue - GPS Odometry Fusion

Question

I am working on estimating a robots pose using Odometry and GPS.

My first problem is that all kinematic model i have seen for a differential drive robot proposes using the displacement of the left and right wheels to evaluate the robots next pose. However, in my situation the robot i have only spits out current X and Y pose relative to the starting point of the movement. can i use this as my state estimate P = [x,y]T

P = [x0,y0] + [dx,dy] where dx and dy are change in respective coordinates gotten from the robots odometry.

if the above is posible how do i calculate the state covariance Q of the filter.

For GPS, how do i evaluate the covariance R; i have tried to collect multiple reading of latitude and longitude from a fixed point but i dont know if this is righ and i just dont get evaluate the covariance from these data (feeling dumb).

Thank you in anticipation.

Answer 1

In regards to your question about differential drive, it sounds as though your robot is using the kinematic model to estimate the position, but this is being done "internally" (i.e., in the embedded on-board software). Whether or not this is okay depends on whether the wheel radii and track width (distance between left and right wheels) it uses are correct. If they aren't (e.g., you changed the wheels), the values it is spitting out will be very inaccurate. Assuming these are approximately correct, instead of the displacement of the left and right wheels, first calculate the translation and rotation since the last measurement, i.e.,

translation = sqrt( (dx - dx_prev)^2 + (dy - dy_prev)^2 )
rotation = dtheta - dtheta_prev

where dx, dy, and dtheta are your current measurements, and dx_prev, dy_prev, dtheta_prev are the previous measurements. Note that I assumed your robot is reporting the heading (theta) as well. If it doesn't, that means if the robot is turning on the spot, you won't be able to tell because no change in position is occurring.

Your new input u is now

u = [translation; rotation]

Use an appropriate kinematic model where this is your input (e.g., change in x is translation * cos(theta), etc.). Setting an appropriate covariance matrix for u is tricky. I would recommend determining it empirically (i.e., do a series of experiments and decide how much uncertainty you should add to u based on the results). Start with a guess such as

Q = [0.005^2, 0; 0, 0.005^2]

and check if the 3-sigma ellipse covers the actual position after the experiment. You can also get fancy and set Q at each time stamp, scaled according to the motion, i.e.,

Q = [(a*translation)^2 + (b*rotation)^2, 0; 0, (c*translation)^2 + (d*rotation)^2]

where you tune for values of a, b, c, and d. This allows you (for example), to add more uncertainty if the robot is turning (larger values of c and d).

For your question about determining the R matrix, without getting into the details of how GPS works, your best bet is probably once again to do tests. One simple test would be to measure out some places on the ground (so you know their positions). Start by converting your initial latitude to (x, y) (see here). This will be your origin. Next drive to each of your known positions and calculate the (x, y) according to the GPS, and subtract off your initial position. Drive from known position to known position and so on (including revisiting positions) and record your (x, y) according to GPS. After this experiment you'll have a list of estimated positions and their corresponding true positions. From this you can use statistics to calculate the average uncertainty, and can set R appropriately.

Good luck!