Q matrix and updating times in a Kalman filter

Question

The context of the problem is that I have several robots located remotely which give their position (x,y coordinates) every x seconds and send it to a centralized remote server. The value of the seconds, x, depends on the update frequency of the robot, so it can be 60, 70, 80 or whatever seconds.

In this server, I take these measurements (x, y coordinates of each robot) and generate a Kalman filter to predict next steps, since the measurements are given each "too much" time and I need these predicted positions.

The Kalman filter has the equations:

x[k] = x[k-1] + v[k-1]*dt

v[k] = v[k-1]

The robots are linked by pairs, and the filter of each robot (inside a pair) is updated when a measurement arrives from any of the two robots.

Here comes the problem. The frequency of update in the filters depends on the time of the robots' measurements. For instance:

• Two robots giving measurements every 10s and every 25s. The filters will be initialized at t=0s and then updated at t=10s, t=20, t=25s, t=30s, etc.

• Two robots giving measurements every 3s and every 5s. The filters will be initialized at t=0s and then updated at t=3s, t=5, t=6s, t=9s, t=10s, etc.

Because of this different updating times, the filters of some pairs of robots work very well, but others do not.

The matrixes Q and R stays constant at any moment. For the Q matrix, the values where given acording to the error produced by the maximum aceleration that can be reached (the problem should be here).

How can I implement a filter (a class) that works for all of the pairs independently of the updating filter times and updated measurement times? Which is the dependence between the update time of the filter and the time when a measurement arrives? I mean, how does it affect quantitatively? Should I modify the matrixes Q, R and/or P according to the time diference? It is an open question since I am a bit lost. Any help would be highly appreciated.

Answer 1

Kalman filter works in a predictor-corrector or predict-update sequence. When no new measurement is available the only thing that you can do is to predict(NOT update) the state with your assumed dynamic model. The prediction is done not only on the state vector itself but also on its covariance matrix ${\bf{P}}$ (Not the measurement covariance matrix $\bf{R}$). A popular model commonly used for model regularization (AKA model noise matrix, or the matrix $\bf{Q}$) is the Singer model from 1990 (see [1]).

In your case the transition matrix is given by $\Phi_k = \begin{bmatrix}1 & dt \\0 & 1 \end{bmatrix}$ , so the state covariance matrix is predicted using ${\bf{P}}_{k|k-1}=\Phi_k {\bf{P}}_{k-1|k-1} \Phi_k^T + {\bf{Q}}_k$. That way the state matrix ${\bf{P}}$ "inflates" to account for the uncertainty.

[1] Singer, R. A., “Estimating Optimal Tracking Filter Performance for Manned Maneuvering Targets,” IEEE Transactions on Aerospace and Electronic Systems, vol. AES-6, no. 4, .pp. 473-483, July 1990.

Answer 2

OK. I've had a go at simulating what I suggested in the comment:

run the KF every 1s then to account for the "missing" data, you can change the $R$ matrix by making it much larger when you don't have a measurement (effectively saying "I don't know what this measurement is")

I've used the code in this question to start with (generating the signal model and doing the full-information Kalman filter). When no measurement exists I've set the measurement to be zero and $R$ to be large (10000).

That yields the plot below. It's a bit hard to see, but the three plots are: the truth (green), the full-information estimate (red), and the missing-information estimate (black).

There are some abrupt jumps in the black estimate, but that is the price for not having full information.

Whether this resolves your issue regarding why different pairs of robots perform differently is anyone's guess. I was expecting the [10,25] measurements to not do very well, but they appear to be doing OK.

---

scilab CODE BELOW

//25865
N = 1000;

t1 = [1:10:N];
t2 = [1:25:N];
t3 = [1:3:N];
t4 = [1:5:N];


// Signal Model
DeltaT = 0.1;
F = [1 DeltaT; 0 1];
G = [DeltaT^2/2; DeltaT];
H = [1 0];

x0 = [0;0];
sigma_a = 0.1;

Q = sigma_a^2;
R = 0.1;

a = rand(1,N,"normal")*sigma_a;

x_truth(:,1) = x0;
for t=1:N,
    x_truth(:,t+1) = F*x_truth(:,t) + G*a(t);
    y(t) = H*x_truth(:,t) + rand(1,1,"normal")*sqrt(R);
end

// Kalman Filter with all data
p0 = 100*eye(2,2);

xx(:,1) = x0;
pp = p0;
pp_norm(1) = norm(pp);
for t=1:N,
    [x1,p1,x,p] = kalm(y(t),xx(:,t),pp,F,G,H,Q,R);
    xx(:,t+1) = x1;
    pp = p1;
    pp_norm(t+1) = norm(pp);
end

clf
plot(xx(1,:),'r') 
plot(H*x_truth,'g');


xx2(:,1) = x0;
pp = p0;
pp_norm(1) = norm(pp);

y_fed = zeros(1,N);

for t=1:N,

    y_now = 0;
    R_now = 10000;

    if (sum(find(t == t1)) > 0)
        y_now = y(t);
        R_now = R;
    end;
    if (sum(find(t == t2)) > 0)
        y_now = y(t);
        R_now = R;
    end;

    y_fed(t) = y_now;

    [x1,p1,x,p] = kalm(y_now,xx2(:,t),pp,F,G,H,Q,R_now);
    xx2(:,t+1) = x1;
    pp = p1;
    pp_norm(t+1) = norm(pp);
end

plot(xx2(1,:),'k')