R matrix at GPSAccKalman.cpp

[Solrac3589]

I have been checking operation by operation inside GPSAccKalman.cpp and Kalman.cpp and I found something which seems to not be critical but technically i think is a mistake and should do the performance of the system t not be ideal .

in this line we are giving the following specifications (it can be seen here ):

• stateDimension = 4 (makes sense, we have 4 states: x,y,xVel and yVel)
• measureDimension = 2 (so I suppose we are considering that the gps is not giving us velocity measures. It's okay).
• controlDimension = 2 (accx and accy. Great).

so, as we can se in the following line, our matrix R will have 2 rows and 2 columns (4 values).

Everything since here is perfect, but if we go in the function on which we are rebuilding R ( here ) we are constructing a matrix of 4 rows and 4 columns! (it is considering velocity as measurement).

Checking how MastrixSet function works ( here ) I think is clear there is a bad construction of the matrix R giving as result something like :

R = [sigmaPos 0 0 0]

While should be:

R = [sigmaPos 0 0 sigmaPos]

If somebody confirms that I am right. I can do a pull request by myself

[Solrac3589]

I have seen ( here ) that you are considering measurement to be 4 (so, velocities also).

Therefor, should be as easy as change the measurement dimensions sended ( here ) by 4 (the second value instead of 2, 4) (well and maybe during R construction do a power of the values).