TransWikia.com

Bias correction for multiple sensor fusion through Kalman Filtering

Robotics Asked on November 28, 2021

I am learning Kalman Filters and was working on a simple example:

Temperature measurement of a room by using 4 thermometers(different biases and noises)

if i consider that there is no bias in the system the model becomes simple with

x = [temp]
F = [1]; H = [1 1 1 1];

but lets assume my thermometers have a bias, how do I incorporate that into my model? How does my x, F and H matrix change?
I am assuming x = [x, B1, B2, B3, B4] but what becomes of F and H?

Here is the code i have written so far.

%a simple kalman test for multiple thermometers reading temp of same item
clc;clear;


N1 = 1;
N2 = 0.5
N3 = 1.5
N4 = 4.5

B1 = -15;
B2 = -13;
B3 = -14;
B4 = -1;

H = [1 0 0 0 ]'; % 0; 1 0 0 0 0; 1 0 0 0 0; 1 0 0 0 0];

F = [1]; % -1 -1 -1 -1; 0 1 0 0 0; 0 0 1 0 0; 0 0 0 1 0; 0 0 0 0 1];
Q = [5] % 0 0 0 0; 0 2 0 0 0; 0 0 2 0 0; 0 0 0 2 0; 0 0 0 0 2]
P = Q;

R = diag([N1, N2, N3, N4]);
x0 = [75]';% B1; B2; B3; B4]

for i = 1:1:100
    T1(i) = 70+N1*randn;%+B1
    T2(i) = 70+N2*randn;%+B2
    T3(i) = 70+N3*randn;%+B3
    T4(i) = 70+N4*randn;%+B4
    
    xobs(:,i) = [T1(i);T2(i);T3(i);T4(i)];
    
    x_est(:,i) = F*x0;

    P = F * P * F' + Q;
    K = P*H'*inv(H*P*H'+R);
    x(:,i) = x0 + K*(xobs(:,i)- H*x0);
    x0 = x(:,i);
end
    
plot(x(1,:));
hold on
plot(T1)
plot(T2)
plot(T3)
plot(T4)
grid
legend('x', 'T1', 'T2', 'T3', 'T4');
hold off

One Answer

Your state vector $x$ is correct.

When you do bias estimation(look up IMU kalman filters) you assume that the bias is constant/slowly varying. So your $F$ matrix is just the identity(5x5).

Your $H$ matrix converts the predicted state into the form of the measurement. So essentially convert this equation into matrix form $T_{est}−B_{x}$ which is just $[1,−1,0,0,0]$ where the -1 will move depending on the sensor.

Also what will probably end up happening is that your sensor with the lowest error/covariance will be taken as the true value, and its bias will tend towards 0.

Answered by edwinem on November 28, 2021

Add your own answers!

Ask a Question

Get help from others!

© 2024 TransWikia.com. All rights reserved. Sites we Love: PCI Database, UKBizDB, Menu Kuliner, Sharing RPP