##### Sections

« February 2020 »
Su Mo Tu We Th Fr Sa
1
2345678
9101112131415
16171819202122
23242526272829

# Unscented Kalman Filter

Don Rosselot's UKF Example Also see 2009 Papers

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)

```