First Order Kalman Filter

Second Order Kanlman Filter

Kalman Filtering

Description

A Kalman filter is a probability-based digital filter capable of filtering "white noise," i.e. noise containing the entire frequency spectrum (or a very wide range in the frequency spectrum). It is related to the Weiner Filter. A basic 1st order implementation often involves smoothing a noisy sensor input for further processing, such as in PID control processing. The first example below shows the results of varying the co-variance variable 'R' in a 1st order filter to obtain a varied amount of smoothing. Higher order filters can be implemented in more complex systems. A 2nd order example is also given.

Additional & Related

Content




1st Order Kalman Filter Example in MatLab

%************************************************************
% Tim Hawkins, Copyright 1999 - 2011 
% 
% Kalman Filter For 1st Order System Sensor Input
% Loops are used in this program for easier portability to C
% or assembly code
%
%************************************************************

clear
load NoisySen.dat  % Actual Noisy Data Taken from the Feedback
                              % from a Temperature Sensor
length = 128;
Q = 1;
R = 10; Adjust R for Degree of Damping

for n = 1: 1: length;
   z(n) = NoisySen(n);
end;

length
Q
R

for n = 1: 1: length;
   index(n) = n;
end;

Pmin1 = 0;

Pmin1
K = Pmin1/(Pmin1+R)

X_hat(1) = NoisySen(1) + K*(z(1) - NoisySen(1))
	
P = (1-K)*Pmin1

Pmin = P + Q

X_hat_min(1) = X_hat(1)

for n = 2: 1: length;
   K = Pmin/(Pmin+R);
   X_hat(n) = X_hat_min(n-1) + K*(z(n) - X_hat_min(n-1));
   P = (1-K)*Pmin;
   Pmin = P + Q;
   X_hat_min(n) = X_hat(n);
end;

figure
plot(index,z,'g-',index,z,'gx',index,X_hat,'b--',index,X_hat,'b*')
legend('Z','Z','X^','X^')
title(['1st Order Kalman Filter Results; R = ',num2str(R)])

%************************************************************

Back to Top


2nd Order Kalman Filter Example in MatLab

%************************************************************
% Tim Hawkins, Copyright 1998 - 2011
% 
% Kalman Filter For 2nd Order System
%************************************************************
clear

load vme.dat
load wws.dat
v_2 = randn(51,1);

T = 0.02;
beta = 1;
sigma = 1;

phi = 2:2;

phi(1,1) = 1;
phi(1,2) = (1 - exp(-beta*T))/beta;
phi(2,1) = 0;
phi(2,2) = exp(-beta*T);

phi

Q = 2:2;
Q(1,1) = 2*(sigma^2)*(T-2*(1 - exp(-beta*T))/
                  beta+(1 - exp(-2*beta*T))/(2*beta))/beta;
Q(1,2) = 2*(sigma^2)*((1 - exp(-beta*T))/
                  beta-(1 - exp(-2*beta*T))/(2*beta));
Q(2,1) = Q(1,2);
Q(2,2) = (sigma^2)*(1 - exp(-2*beta*T));

Q

H = 2:2;

H(1,1) = 1;
H(1,2) = 0;
H(2,1) = 0;
H(2,2) = 1;

H

R = 2:2;

R(1,1) = 0.25;
R(1,2) = 0;
R(2,1) = 0;
R(2,2) = 1;

R

x2(1) = sqrt(Q(2,2))*wws(1);

for n = 2: 1: 51;
	x2(n) = phi(2,2)*x2(n-1) + sqrt(Q(2,2))*wws(n);
end;

x1(1) = 0 + x2(1)*T;

for n = 2: 1: 51;
	x1(n) = x1(n-1) + x2(n)*T;
end;

for n = 1: 1: 51;
	z_1(n) = x1(n) + 0.5*vme(n);
end

for n = 1: 1: 51;
	z_2(n) = x2(n) + v_2(n);
end
z = 2:1;
for n = 1: 1: 51;
	z(1,n) = z_1(n);
	z(2,n) = z_2(n);
end

for n = 1: 1: 51;
	index(n) = n;
end;

Pmin1 = 2:2;
Pmin = 2:2;
P = 2:2;
Pmin1(1,1) = 10;
Pmin1(1,2) = 0;
Pmin1(2,1) = 0;
Pmin1(2,2) = 10;

Pmin1

I = eye(2);
X_hat = 2:1;
X_hat_plus = 2:1;

K = Pmin1*H'/(H*Pmin1*H'+R);
k_1(1) = K(1,1);
k_2(1) = K(2,1);

X_hat = 0 + K*(z(1) - H*0);
	x1_hat(1) = X_hat(1,1);
	x2_hat(1) = X_hat(2,1);
	
P = (I-K*H)*Pmin1;
Pmin = phi*P*phi' + Q;
	P_11(1) = P(1,1);
	P_22(1) = P(2,2);
X_hat_plus = phi*X_hat;

for n = 2: 1: 51;

K = Pmin*H'/(H*Pmin*H'+R);
	k_1(n) = K(1,1);
	k_2(n) = K(2,1);
X_hat = X_hat_plus + K*(z(n) - H*X_hat_plus);
	x1_hat(n) = X_hat(1,1);
	x2_hat(n) = X_hat(2,1);
	
P = (I-K*H)*Pmin;
	P_11(n) = P(1,1);
	P_22(n) = P(2,2);
Pmin = phi*P*phi' + Q;
X_hat_plus = phi*X_hat;

end;

figure
plot(index,x1,'r-',index,x1,'ro',index,x1_hat,'m:',index,
               x1_hat,'mx',index,z_1,'g:',index,z_2,'y:',
                index,x2,'c-',index,x2,'c*',index,x2_hat,
                'b:',index,x2_hat,'b+')
legend('X1','X1','X1^','X1^','Z1','Z2','X2','X2','X2^',
                                                    'X2^')
title('2nd Order Kalman Filter Results')

figure
plot(index,P_11,'r--',index,P_11,'ro',index,P_22,'b-.',
                                         index,P_22,'bx')
legend('P_11','P_11','P_22','P_22')
title('2nd Order Kalman Filter Results')

figure
plot(index,x1,'r-',index,x1,'ro',index,x1_hat,
     'm:',index,x1_hat,'mx',index,z_1,'g:',index,z_1,'g*')
legend('X1','X1','X1^','X1^','Z1','Z1')
title('2nd Order Kalman Filter Results')

figure
plot(index,x2,'r-',index,x2,'ro',index,x2_hat,'m:',
            index,x2_hat,'mx',index,z_2,'g:',index,z_2,'g*')
legend('X2','X2','X2^','X2^','Z2','Z2')
title('2nd Order Kalman Filter Results')

%************************************************************

Back to Top