1
$\begingroup$

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.

$\endgroup$
  • $\begingroup$ In the absence of a new measurement from one of the robots, you can still update its Kalman filter to predict its position/velocity at the next time step. This will really just be equivalent to dead reckoning based on the previous state estimates, but it can be useful in some applications. $\endgroup$ – Jason R Sep 16 '15 at 11:22
  • $\begingroup$ Thas is what I am doing. The problem is that the filters of some pairs of robots work, but others do not work. The reason lies in the diference among filters in the updating times of the filters and the updating times of the measurements. I mean: some filters are updated each 3 seconds and the measurements are received every 5 seconds; other filters every 2s and 6s; others every 7s and 10s (for instance). So the real problem is that I do not know a Q matrix that adapt to this difference in times. More clear? I am sorry, but it's difficult to explain! :) $\endgroup$ – YisasL Sep 16 '15 at 11:33
  • $\begingroup$ There are a few things that might be the problem. First, is there a relation between the update time differences (3 vs 5 vs 10) and whether they work or not? It could be that the longer ones are not sampling fast enough. Second, if you 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"). $\endgroup$ – Peter K. Sep 16 '15 at 12:35
1
$\begingroup$

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.

$\endgroup$
0
$\begingroup$

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.

enter image description here


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') 
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.