multi-sensors fusion using Kalman filter - kalman-filter

Question

I need to use the Kalman filter to fuse multi-sensors positions for gaussian measurement (for example 4 positions as the input of the filter and 1 position as output). It is possible to help me with some examples or tutorials because all the examples I found are related to the estimation of the positions?

Answer 1

OPTION 1
Weighted Avarage
In this case you don't need to implement a real Kalman Filter. You just can use the signal variances to calculate the weights and then calculate the weighted avarage of the inputs. The weights can be found as an inverse of the variances.
So if you have two signals S1 and S2 with variances V1 and V2, then the fused result would be
A fusion example can be seen on the next plot.
I simulated two signals. The variance of the second signal changes over the time. As long as it's smaller than the variance of the first signal the fused result is close to the second signal. It is not the case when the variance of the second signal is too high.
OPTION 2
Kalman Filter with Multiple Update Steps
The classical Kalman Filter uses prediction and update steps in a loop:
prediction
update
prediction
update
...
In your case you have 4 independent measurements, so you can use those readings after each other in separate update steps:
prediction
update 1
update 2
update 3
update 4
prediction
update 1
...
A very nice point is that the order of those updates does not matter! You can use updates 1,2,3,4 or 3,2,4,1. In both cases you should get the same fused output.
Compared to the first option you have following pros:
You have a variance propogation
You have the system noise matrix Q,
so you can control the smoothness of the fused output
Here is my matlab code:
function [] = main()
% time step
dt = 0.01;
t=(0:dt:2)';
n = numel(t);
%ground truth
signal = sin(t)+t;
% state matrix
X = zeros(2,1);
% covariance matrix
P = zeros(2,2);
% kalman filter output through the whole time
X_arr = zeros(n, 2);
% system noise
Q = [0.04 0;
0 1];
% transition matrix
F = [1 dt;
0 1];
% observation matrix
H = [1 0];
% variance of signal 1
s1_var = 0.08*ones(size(t));
s1 = generate_signal(signal, s1_var);
% variance of signal 2
s2_var = 0.01*(cos(8*t)+10*t);
s2 = generate_signal(signal, s2_var);
% variance of signal 3
s3_var = 0.02*(sin(2*t)+2);
s3 = generate_signal(signal, s3_var);
% variance of signal 4
s4_var = 0.06*ones(size(t));
s4 = generate_signal(signal, s4_var);
% fusion
for i = 1:n
if (i == 1)
[X, P] = init_kalman(X, s1(i, 1)); % initialize the state using the 1st sensor
else
[X, P] = prediction(X, P, Q, F);
[X, P] = update(X, P, s1(i, 1), s1(i, 2), H);
[X, P] = update(X, P, s2(i, 1), s2(i, 2), H);
[X, P] = update(X, P, s3(i, 1), s3(i, 2), H);
[X, P] = update(X, P, s4(i, 1), s4(i, 2), H);
end
X_arr(i, :) = X;
end
plot(t, signal, 'LineWidth', 4);
hold on;
plot(t, s1(:, 1), '--', 'LineWidth', 1);
plot(t, s2(:, 1), '--', 'LineWidth', 1);
plot(t, s3(:, 1), '--', 'LineWidth', 1);
plot(t, s4(:, 1), '--', 'LineWidth', 1);
plot(t, X_arr(:, 1), 'LineWidth', 4);
hold off;
grid on;
legend('Ground Truth', 'Sensor Input 1', 'Sensor Input 2', 'Sensor Input 3', 'Sensor Input 4', 'Fused Output');
end
function [s] = generate_signal(signal, var)
noise = randn(size(signal)).*sqrt(var);
s(:, 1) = signal + noise;
s(:, 2) = var;
end
function [X, P] = init_kalman(X, y)
X(1,1) = y;
X(2,1) = 0;
P = [100 0;
0 300];
end
function [X, P] = prediction(X, P, Q, F)
X = F*X;
P = F*P*F' + Q;
end
function [X, P] = update(X, P, y, R, H)
Inn = y - H*X;
S = H*P*H' + R;
K = P*H'/S;
X = X + K*Inn;
P = P - K*H*P;
end
And here is the result: