# Why can't I add variable covariance noise to this 2D kalman filter?

The goal is to include variable measurement noise to my kalman filter. To begin with I kept the noise covariance matrix as follows: $$R=\begin{bmatrix}1&0&0&0\\ 0&1&0&0\\ 0&0&1&0\\ 0&0&0&1\end{bmatrix}$$

This gave good results, the Kalman filter did a good job following the input data.

Next I tried to calculate the measurement noise covariance myself: https://pastebin.com/7ETMC0Fy

Which gave such results:

Could someone tell me why when I try to calculate my measurement noise covariance matrix myself the result is unsatisfactory? Why doesn't the output of my Kalman filter follow the input data at all?

• ...matplots figure functionality has an "export image" tool. Use that instead of posting screenshots. – Marcus Müller Apr 5 '17 at 9:18

x = detrend([1, 5, 10, 15, 20,  1, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80]);
y = detrend([1, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80]);


do:

x = [1, 5, 10, 15, 20,  1, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80];
y = [1, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80];
dx = detrend(x);
dy = detrend(y);


and then:

  myMeanX = mean(dx);
myMeanY = mean(dy);
varianceX = 0;
varianceY = 0;
for i=1:length(dx)
varianceX = varianceX + (myMeanX - dx(i))^2;
varianceY = varianceY + (myMeanY - dy(i))^2;

end


and

R(1,1) = varianceX/length(dx);
R(2,2) = varianceY/length(dy);


and I'm not sure why you're doing this:

%set everything to zero except diagonal


P should be as-calculated. No need to zero out entries.

The code you show must be wrong because:

[31.5202     0     0    0
0       0.0461  0    0
0       0.0458  0    0
0        0      0    0.0051]


shows you are setting off-diagonal entries in R.

Compare:

varianceX = varianceX + (myMeanX - x(i))^2;


with

varianceVx = varianceVx + (myMeanVx - VxMeas(i));


Notice anything²?

Also, it would be good if you could actually print out the values of your estimated R matrix.

² $\tiny \mbox{If you don't, please read it again.}$

• Hi, thank your for your answer! Indeed I added a power of 2 (already tried that in the past), but the result still doesn't look as I expected it: imgur.com/a/uV9A0 Updated code: pastebin.com/R76tzrsp This didn't solve the issue, any idea? – wgujwhzi Apr 5 '17 at 8:32
• @wgujwhzi What are the values of R when you estimate? Something is wrong as the Kalman gain should, in general, be going down not up. :-) – Peter K. Apr 5 '17 at 8:37
• I really don't know what the issue is, I totally understand what you say and mean and why it is like that. But I just don't see what I am doing wrong. results of R: pastebin.com/GkSme6z8 – wgujwhzi Apr 5 '17 at 8:42
• @wgujwhzi Thanks. Those values are all over the shop, probably because the x and y values are linear. I'd suggest using detrend before finding the variance. Otherwise you're going to get very large variance values. – Peter K. Apr 5 '17 at 9:24
• Im affraid this made it much worse... Or did I misunderstand you? imgur.com/a/F9go4 , value of R: pastebin.com/tXwtHh7v , code: pastebin.com/ane2NqFj My measurements look kind of like nonsense now and so does the kalman output. (if that helps, I tried to implement the movement of a real object with those xy coordinates. It's an object moving from the left to the right but at some time the measurement of the position is incorrect. The error was represented by that "spike" in the linear line) – wgujwhzi Apr 5 '17 at 9:33