Personal tools
Navigation
Log in


Forgot your password?
« November 2019 »
Su Mo Tu We Th Fr Sa
12
3456789
10111213141516
17181920212223
24252627282930
 
Document Actions

Unscented Kalman Filter

by hallel — last modified 2009-11-19 09:42

Don Rosselot's UKF Example Also see 2009 Papers

Click here to get the file

Size 5.1 kB - File type text/x-objcsrc

File contents

function  UKalFilter(t) % t is number of time steps
  
clear;
clc;
close all;

t = 500;
T = 1:t;                % The time axis

n=2;                    % Dimention of X
f = 221.58              % Focal length constant
B = .119562             % Base line constant
W = 1/(2*n);            % The weight factor

% Assume position is measured (with noise) at X1=5, X2=6
% Dummy data 
d = 17.5;       % Dummy disparity (trying to wind up with object at Z=1.5, X=1)
u = 147.7       % Dummy of u (x position of object in u-v space) 
Z1_d = d*ones(t,1) + d*0.1*rand(t,1);  % Measurement signal of disparity with noise
Z2_u = u*ones(t,1) + u*0.1*rand(t,1);  % Measurement signal of u (x position of object in u-v space) with noise

ZZ = (f*B)./Z1_d;  % Position in Z with noise
XX = (Z2_u.*ZZ)/f;  % Position in X with noise

X1 = zeros(t,1);
X2 = zeros(t,1);
X1(1) = 1.46;           % Initialize Z position
X2(1) = .95;          % Initialize X position

Xmean = zeros(1,2);

format short e
Pyy = [ .01 0; 0 .01 ]  % The Covariance matrix
Pk = Pyy;             % Init to Pyy 
Q = 1e-5;     % Initialize process noise covariance
% R = .01;      % Initialize measurement noise covariance
R = .1;      % Initialize measurement noise covariance
format

Xi = zeros(2,4);        % Sigma points
Y1 = zeros(4,1);        % Predicted Measurement 1
Y2 = zeros(4,1);        % Predicted Measurement 2
Yk  = zeros(2,1);       % Actual Measurements (d and u)

for k = 2:t
    
	% Begin State Model
    % Update time equations.  Do not use sigma points because the state model is linear
	X1(k) = X1(k-1);                  % Project the state ahead
	X2(k) = X2(k-1);                  % Project the state ahead
    
    Xk = [X1(k); X2(k)];

    % Begin Observation Model
    % Create the sigma points around Xk 
	sqrtNP = (sqrt(n*Pk));                  % Get transpose of square root of n*P
	sqrtNP1 = sqrtNP(1,:)' ;                  % First column 
	sqrtNP2 = sqrtNP(2,:)';                 % Second column
	
	Xi(:,1) = Xk + sqrtNP1;
	
	Xi(:,2) = Xk + sqrtNP2;
	
	Xi(:,3) = Xk - sqrtNP1;
	
	Xi(:,4) = Xk - sqrtNP2;
    
    % Get XX covariance Pk
    Ztmp = 0;
    Xtmp = 0;
    for i = 1:2*n
      Ztmp = Ztmp + (Xi(1,i) - Xk(1,1))^2;
      Xtmp = Xtmp + (Xi(2,i) - Xk(2,1))^2;
    end
    
    Pk = [ Ztmp*W + Q 0 ; 0 Xtmp*W + Q ];

    % Transform sigma points into Y vectors (predicted measurements)
    for i = 1:2*n
        Y1(i) = (f*B)/Xi(1,i);          % d=(f*B)/Z     Xi is a vector [Z;X}
        Y2(i) = (Xi(2,i)*f)/Xi(1,i);      % u=(X*f)/Z
    end
    
    
    % Combine the Y vectors to obtain the predicted measurement at time k
    Yp1 = sum(Y1)*W;    % d predicted (target 17.5)
    Yp2 = sum(Y2)*W;    % u predicted (target 147.7)
    
    % Get YY covariance
    Ztmp = 0;
    Xtmp = 0;
    for i = 1:2*n
      Ztmp = Ztmp + (Y1(i) - Yp1)^2;
      Xtmp = Xtmp + (Y2(i) - Yp2)^2;
    end
    
    % Estimate the covariance of the predicted measurement
    Pyy = [ Ztmp*W  + R 0 ; 0 Xtmp*W + R ];

    %Get XY covariance
    Ztmp = 0;
    Xtmp = 0;
    for i = 1:4
      Ztmp = Ztmp + abs((Xi(1,i) - Xk(1,1)))*abs((Y1(i) - Yp1));
      Xtmp = Xtmp + abs((Xi(2,i) - Xk(2,1)))*abs((Y2(i) - Yp2));
    end

    % Estimate the cross covariance between x and y predicted
    Pxy = [ Ztmp*W  0 ; 0 Xtmp*W ];

    % Get measurement (u and d) data and 
    Yk(1) = Z1_d(k);    %   disparity 17.5 with noise
    Yk(2) = Z2_u(k);    %   u pixel 147.7 (x pixel) with noise
    
    % Update measurement equations
	Kk    = Pxy/(Pyy)  ;                     % Compute the Kalman gain
 	X1(k) = X1(k) + Kk(1,1)*(Yp1-Yk(1));   % Update estimate of Xk with measurement Z (Dummy data)
    X2(k) = X2(k) + Kk(2,2)*(Yk(2) - Yp2);    % Update estimate of Xk with measurement Z (Dummy data)
	Pk    = Pk - Kk*Pyy*Kk'   ;             % Update error covariance
    
%     if (mod(k,20) == 0)   % For debugging 
%         jj = 1;
%     end    

end

X1mean = mean(ZZ)
X1mean_after = mean(X1)
X1std = std(ZZ)
X1std_after = std(X1)


Z1mean = mean(XX)
Z1mean_after = mean(X2)
Z1std = std(XX)
Z1std_after = std(X2)

figure(1);
tt = 100
X1meanArray = ones(tt)* X1mean;
Z1meanArray = ones(t)* Z1mean;
% X1(:,1:tt)
% X1(1:tt,:)
% ZZ(1:tt,:)
plot(1:tt,X1(1:tt,:),1:tt,ZZ(1:tt,:),1:tt,X1meanArray,'r');        
% axis([0 t 1.36 1.52]);
% title('Z position before and after filtering with UKF');

X1meanArray = ones(t)* X1mean;

figure(2);
plot(T,X1,T,ZZ,'.',T,X1meanArray,'r');
axis([1 t 1.37 1.52]);
% title('Z position after filtering with UKF');

% figure(3)
% plot(T,ZZ);
% axis([0 t 1.36 1.52]);
% title('Z position before filtering with UKF');

figure(3);
plot(T,X2,T,XX,'.',T,Z1meanArray,'r');
axis([1 t .92 1.12]);
% title('X position after filtering with UKF');


% figure(5)
% plot(T,XX);
% axis([0 t .9 1.14]);
% title('X position before filtering with UKF');

X1mean = mean(ZZ)
X1mean_after = mean(X1)
X1std = std(ZZ)
X1std_after = std(X1)


Z1mean = mean(XX)
Z1mean_after = mean(X2)
Z1std = std(XX)
Z1std_after = std(X2)