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
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
Get help from others!
Recent Questions
Recent Answers
© 2024 TransWikia.com. All rights reserved. Sites we Love: PCI Database, UKBizDB, Menu Kuliner, Sharing RPP