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

Question

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?

Answer 1

Instead of:

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.}$