2. Kalman Filtering
Introduction
Kalman Filter uses a series of measurements over time, and produces estimates of unknown
variables that tend to be more precise than those based on a single measurement alone. It
operates recursively on streams of noisy input data, to produce a statistically optimal estimate of the
underlying system state. It’s a two step process, in first phase estimates of current state variables with
their uncertainties are calculated, while in second part these estimates are updated using weighted average
after observing output. It operates on real time data, no additional past information is required. It’s
different from wiener filter in processing in sense that it can operate on non-stationary data while wiener
filter assumes data to be stationary. It allows the parameters to be estimated to evolve with time according
to a dynamical model.
To study Kalman filter in detail, we must have knowledge of the dynamical signal modeling. An
introduction to dynamical signal models is given in following section.
Dynamical Signal Models
We begin our discussion of signal modeling by using an example of a DC voltage source in
presence of AWGN.
(1)
Fig. 1 True voltage and MVU estimator.
3. where, A is the parameter to be estimated and
is the noise introduced by the inaccurate voltmeter. In
real systems parameter is not a constant, in fact it varies with time with small variance due to the
component aging, temperature and other effects. So we can change the signal model to be as
(2)
here,
is also a function of time, i.e., it varies with time. Successive samples of
will not be too
different, leading us to conclude that they yield a high degree of correlation. The problem is to estimate
correlated process
using noisy observations x . Fig. 1 shows the corresponding graphs.
Gauss Markov Process
The correlated process
can be modeled using 1st order Gauss Markov Process. Gauss
Markov Process is a simple model to describe correlation between samples. Fig. 2 shows the modeling of
to generate Gauss Markov Process s . Equations [3-5] show the generation process of scalar state Gauss
Markov Process, its recursive form and mean of the process.
(3)
(4)
(5)
where,
is the deriving noise having variance
Process can be summarized as
2
. In vector form the vector state Gauss Markov
(6)
(7)
Fig. 2 Dynamical Signal Model used to generate Gauss Markov Process
Kalman filter can be portioned into 4 main categories. These include following;
i)
ii)
iii)
iv)
Scalar state scalar Kalman filter.
Scalar state vector Kalman filter.
Vector state vector Kalman filter.
Extended Kalman filter.
4. Scalar state scalar Kalman filter
Gauss Markov Model described in previous section has the form
We now describe a sequential MMSE estimator which will allow us to estimate
based on the data
as
increases. Such an operation is known as Kalman filtering. The approach
computes the estimator
based on the previous time sample
and so is recursive is in nature.
This is so called Kalman filter. Fig. 3 shows the model of the scalar state scalar Kalman filter.
Fig. 3 Kalman filter
Mathematically we can describe the filtering process as;
Prediction:
Minimum Prediction MSE:
Kalman Gain:
Correction:
Minimum MSE
5. Simulation setup
To generate the 1st order Gauss Markov Process, the noise variance of the deriving noise
is
assumed to be 0.1 and the noise introduced by the inaccurate voltmeter was assumed to be of variance 1.
We can model our problem as calculating the gain of a random channel i.e. predicting one state only
depending upon the previous samples. The correlation coefficient in generating Gauss Markov Process
is assumed to be 0.1. Number of samples transmitted was taken to be 100. Fig. 4 shows the results
against number of samples. Figs. [4-5] shows the results obtained for scalar Kalman filter.
Fig. 4
Scalar state Vector Kalman Filter
For the Scalar state Vector Kalman Filter, we have multiple states
observation vector
. In this case the model is shown to be as
and single realization of
6. Fig. 5
where
are known
and
matrices,
is vector WGN.
The data model is observation or measurement equation. The Kalman filter derived for this setup is
exactly the same as for the scalar case; we summarize the results.
Prediction
Minimum Prediction MSE Matrix (p*p):
Kalman Gain Vector (P*1):
Correction:
7. Minimum MSE Matrix (p*p):
Fig. 6
Vector state Vector Kalman Filter
For the vector state Vector Kalman Filter, we have multiple states
of observation vector
. In this case the model is shown to be as
where
are known
and
matrices,
and multiple realizations
is vector WGN.
where
is
known observation matrix. The data model is observation or measurement
equation. The Kalman filter derived for this setup is exactly the same as for the scalar case; we summarize
the results.
Prediction
Minimum Prediction MSE Matrix (p*p):
Kalman Gain Vector (P*1):
Correction:
8. Minimum MSE Matrix (p*p):
Fig. 7 shows the simulation result for the vector state vector Kalman filter. In this example particle
tracking problem is considered, having two states the x-position & y-position.
Fig. 7 Vector Kalman Filter
Extended Kalman Filter
Vector state vector Kalman filter can be modified to describe the model of extended Kalman
filter, in which state equation or observation equation may be non-linear. A simple example is considered
here, that is of vehicle tracking, in which the observations are range estimates and bearing estimates.
For the present case, the Gauss Markov Process and the observation or measurement is given as non-liner
function of the state variables.
s[ n ] a ( s[ n 1]) Bu [ n ]
x[ n ] h ( s[ n ]) w[ n ]
9. ˆ
a ( s[ n 1]) a ( s [ n 1 | n 1])
A[ n 1]
a
s[ n 1]
h
s[ n ]
s[ n 1]
| s [ n 1 ] sˆ [ n 1| n 1 ]
| s [ n 1 ] sˆ [ n 1| n 1 ]
ˆ
h ( s[ n ]) h ( s [ n | n 1])
H [n]
a
h
s[ n ]
| s [ n ] sˆ [ n | n 1 ]
| s [ n ] sˆ [ n 1| n 1 ]
Prediction:
ˆ
ˆ
s[ n | n 1] a ( s[ n 1 | n 1])
Minimum Prediction MMSE
M [ n | n 1] A[ n 1] M [ n 1 | n 1] A [ n 1] BQB
T
Kalman Gain
M [ n | n 1] H [ n ]
T
K [n]
C [ n ] H [ n ] M [ n | n 1] H [ n ]
T
Correction
ˆ
ˆ
ˆ
s[ n | n ] s[ n | n 1] K [ n ]( x[ n ] h ( s[ n | n 1]))
Minimum MSE
M [ n | n ] ( I K [ n ] H [ n ]) M [ n | n 1]
Fig. 8 shows the particle track using extended Kalman filter.
T
10. Fig. 8 Extended Kalman Filter
Appendix: Matlab Code;
Scalar Kalman Filter
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PARTICLE TRACKING
% USING SCALER KALMAN FILTER
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N = 100; %number of samples
sig_u_sqr = 0.1; % driving noise (control)
sig_x_sqr = 0.1; % noise variance of range observation
%initialization
s = zeros(1, N);
x = zeros(1, N);
pre_est_s = zeros(1,N);
est_s = zeros(1,N);
M = zeros(1,N);
pre_M = zeros(1,N);
K = zeros(1,N);
a=1;
11. %initial condiitions
est_s(1) = 5;
s(1) = 10;
M(1) = 100;
for n=2:N
%particle is moving with constant velocity "1",
%with driving noise of sig_u_sqr variance
s(n) = a*s(n-1) + sig_u_sqr*randn(1,1);
%observing the position with noise variance sig_x_sqr
x(n) = s(n) + sig_x_sqr*randn(1,1);
%Prediction
pre_est_s(n) = est_s(n-1);
%Minimum Prediction MSE
pre_M(n) = a^2*M(n-1) + sig_u_sqr;
%Kalman Gain
K(n) = pre_M(n)/(sig_x_sqr + pre_M(n));
%Correction
est_s(n) = pre_est_s(n) + K(n)*(x(n) - pre_est_s(n));
%Minimum MSE
M(n) = (1 - K(n))*pre_M(n);
end
%%
figure
plot(s,'-r')
hold on
plot(x(2:end), '-b')
plot(est_s, '-k')
hold off
legend('Actual track', 'Observed track', 'Estimated track')
xlabel('Samples')
ylabel('X position')
title('Particle track')
grid on
figure;
plot(M); title('MMSE for scalar Kalman filter');
xlabel('No. of samples');
ylabel('MMSE');
grid on
Vector Kalman Filter:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PARTICLE TRACKING
% USING VECTOR KALMAN FILTER
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
del = 1; %time step
N = 100; %number of samples
12. sig_u_sqr = 0.001; %driving noise (control)
sig_x_sqr = 0.1; %noise variance of range observation
sig_y_sqr = 0.1; %noise variance of bearing observation
A = [ 1,
0,
0,
0,
0,
1,
0,
0,
del,
0 ,
1 ,
0 ,
B = [ 0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
1,
0,
0 ;
del;
0 ;
1 ];
0;
0;
0;
1];
%linear transformation of observation
H = [ 1, 0, 0, 0;
0, 1, 0, 0];
%covariance
Q = [ 0, 0,
0, 0,
0, 0,
0, 0,
matrix of system
0
, 0
;
0
, 0
;
sig_u_sqr, 0
;
0
, sig_u_sqr];
%convarince matrix of observation
C = [ sig_x_sqr, 0
;
0
, sig_y_sqr];
%initialization
s = zeros(4, N);
x = zeros(2, N);
pre_est_s = zeros(4,N);
est_s = zeros(4,N);
M = zeros(4,4,N);
pre_M = zeros(4,4,N);
K = zeros(4,2,N);
%initial values
est_s(:,1) = [5;5;0;0];
s(:,1) = [10;-5;-0.2;0.2];
M(:,:,1) = eye(4)*100;
I = eye(4);
for n=2:N
%driving noise vector with same x and y noise variances
u = [0;0;sig_u_sqr*randn(1,1);sig_u_sqr*randn(1,1)];
%particle is moving in x-y plane
s(:,n) = A*s(:,n-1) + B*u;
%observing noise in x and y position
w = [sig_x_sqr*randn(1,1);sig_y_sqr*randn(1,1)];
%obseration of x and y coordinates with noise
x(:,n) = H*s(:,n) + w;
13. %Prediction
pre_est_s(:, n) = A*est_s(:, n-1);
%Minimum Prediction MSE Matrix
pre_M(:,:,n) = A*M(:,:,n-1)*A' + B*Q*B';
%Kalman Gain
K(:,:,n) = pre_M(:,:,n)*H'*(C + H*pre_M(:,:,n)*H')^-1;
%Correction
est_s(:,n) = pre_est_s(:,n) + K(:,:,n)*(x(:,n) - H*pre_est_s(:,n));
%Minimum MSE Matrix
M(:,:,n) = (I - K(:,:,n)*H)*pre_M(:,:,n);
end
figure
plot(s(1,:), s(2,:))
hold on
plot(x(1,:), x(2,:), ':g')
plot(est_s(1,:), est_s(2,:), '--r')
hold off
legend('Actual track', 'Observed track', 'Estimated track')
xlabel('X position')
ylabel('Y position')
title('Particle track')
grid on
Extended Kalman Filter:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PARTICLE TRACKING
% USING EXTENDED KALMAN FILTER
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc
clear
close all
%%
del = 1; %time step
N = 100; %number of samples
sig_u_sqr = 0.001; %driving noise (control)
sig_r_sqr = 0.1; %noise variance of range observation
sig_b_sqr = 0.1; %noise variance of bearing observation
A = [ 1,
0,
0,
0,
0,
1,
0,
0,
%convarince
Q = [ 0, 0,
0, 0,
0, 0,
0, 0,
del,
0 ,
1 ,
0 ,
0 ;
del;
0 ;
1 ];
matrix of driving noise
0
, 0
;
0
, 0
;
sig_u_sqr, 0
;
0
, sig_u_sqr];
14. %covarince matrix of observation noise
Cw = [ sig_r_sqr, 0
;
0
, sig_b_sqr];
%initializations
s = zeros(4, N);
x = zeros(2, N);
pre_est_s = zeros(4,N);
est_s = zeros(4,N);
M = zeros(4,4,N);
pre_M = zeros(4,4,N);
K = zeros(4,2,N);
%initial values
est_s(:,1) = [5;5;0;0];
s(:,1) = [10;-5;-0.2;0.2];
M(:,:,1) = eye(4)*100;
I = eye(4);
%%
for n=2:N
%particle moving in x y plane
s(:,n) = A*s(:,n-1) +
[0;0;sig_u_sqr*randn(1,1);sig_u_sqr*randn(1,1)];
%observing range and bearing of the particle with noise
x(:,n) = [sqrt(s(1,n)^2+s(2,n)^2);atan2(s(2,n),s(1,n))] + ...
[sig_r_sqr*randn(1,1);sig_b_sqr*randn(1,1)];
%Prediction
pre_est_s(:, n) = A*est_s(:, n-1);
%Minimum Prediction MSE Matrix
pre_M(:,:,n) = A*M(:,:,n-1)*A' + Q;
%extracting rx ry and range from Prediction
rx = pre_est_s(1,n);
ry = pre_est_s(2,n);
R = sqrt(rx^2 + ry^2);
%creating H from previously extracted rx ry and range (R)
H = [rx/R
, ry/R , 0, 0;
-ry/R^2, rx/R^2, 0, 0];
%Kalman Gain
K(:,:,n) = pre_M(:,:,n)*H'*(Cw + H*pre_M(:,:,n)*H')^-1;
%creating h from previously extracted rx ry and range (R)
h = [sqrt(rx^2 + ry^2);atan2(ry,rx)];
%Correction
est_s(:,n) = pre_est_s(:,n) + K(:,:,n)*(x(:,n) - h);
%Minimum MSE Matrix
M(:,:,n) = (I - K(:,:,n)*H)*pre_M(:,:,n);
end
%%
figure
plot(s(1,:), s(2,:))
hold on
15. plot(est_s(1,:), est_s(2,:), '--r')
hold off
legend('Actual track', 'Estimated track')
xlabel('X position')
ylabel('Y position')
title('Particle track')
grid on
figure
plot(x(1,2:end))
legend('Observed range')
xlabel('Samples')
ylabel('Range')
grid on
figure
plot(x(2,2:end))
legend('Observed bearing')
xlabel('Samples')
ylabel('Bearing in Radians')
grid on
