The Kalman Filter
Kristof Van Laerhoven
The Kalman filter
The Kalman filter is an on-line, recursive algorithm trying to estimate the true state of a system where only (noisy) observations or measurements are available.
In Bayesian terms, we want to propagate the conditional probability density of the true state, given knowledge on previous measurements.
The discrete Kalman filter
The Kalman filter contains a linear model for the process, as well as a linear model for the measurement. The former model describes how the current state of the tracker is changing, given the previous instance:
or in a shorter notation:
with A the state transition matrix and ~p the process noise vector.
The measurement model of the filter is straightforward as well:
or shorter: 
with H the measurement matrix and ~m the measurement noise vector. H described how the measured data relates (linearly) to the state.
We define
and
as the covariance matrices of the process and measurement noise respectively.
We start in the initial state:
and define
, with e small
and update the Kalman variables in a two-step predict-correct loop, until we run out of data:
Predict:
Predict the next state:
(a priori)
Predict the next error
covariance: 
Correct using the measurements:
Compute Kalman gain:
Update estimated state with
measurement: 
Update the error covariance:
Example (in Matlab)
% simple kalman filter example
% A is the input:
%eg:
A = textread('C:\Documents and Settings\vl\My
Documents\object-C_m.txt');
DIM = size(A,2);
LEN = size(A,1);
% this is the output:
S = [];
SS = [];
% assume measurement matrix H is identical:
H = eye(DIM);
% and the state transition matrix T as well:
T = eye(DIM);
% init
p = H*A(1,:)';
P = eye(DIM)*10; % just an initial guess
Q = eye(DIM);
R = eye(DIM)*150;
% loop for the entire dataset:
for i=2:LEN,
% estimate step:
p_est = T*p;
P_est = T*P*T'+Q;
% correction step:
K = P_est*H'*inv(H*P_est*H'+R);
p = p_est + K*( A(i,:)' -
H*p_est );
P = (eye(DIM) - K*H)*P_est;
S = [S p];
SS = [SS; P(1,1) P(1,2)
P(2,1) P(2,2)];
end
S = S';
subplot(2,1,1);
plot(A(2:length(A),:));
subplot(2,1,2);
plot(S);
References