Void’s Vault

Knowledge source for efficiency.

Unscented Kalman Filter

I was reading a book from Thrun, Burgard and Fox called "Probabilistic Robotics", when I decided to implement some filtering algorithms found in the book. So here you will find the maltab script to implement an unscented Kalman Filter.

Let’s suppose we have a robot moving toward the first quarter of a planar region. It will move a non linear way along X and Y, and it only have one non linear infrared sensor to detect the distance between itself and the origin of the planar region. Because there’s nothing linear in this problem, we cannot use the regular Kalman Filter to find our robot’s real position.

We define the following variables, that will be used in the UKF function: mu0: The current estimation of the robot’s position. 2X1 matrix. sigma0: The current error on the estimated position. 2x2 matrix. u1: The robot’s movement. 2X1 matrix. z1: The measurement after u1. 1X1 matrix. x-y-zErr: The error on x, y and z, respectively.

Enjoy.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
function [ mu1, sigma1 ] = ukf( mu0, sigma0, u1, z1, xErr, yErr, zErr)
% The unscented Kalman Filter algorithm. 
n = numel(mu0); % Dimensionality of the state vector

alpha = .001;   % Scaling parameter, default 0.001
kappa = 0;      % Scaling parameter, default 0
lambda = (alpha.^2 * (n + kappa)) - n;
gamma = sqrt(n + lambda);

% beta parameter can be chosen to encode additionnal (higher order)
% knowledge about the distribution underlying the Gaussian representation
% beta = 2 is optimal if distribution is an exact Gaussian.
beta = 2;

% Each sigma points has two associated weights
wm = zeros(1,2*n+1); % wm is used when computing the mean
wc = zeros(1,2*n+1); % wc is used recovering the covariance of the Gaussian

wm(1) = lambda / (lambda + n);
wc(1) = lambda / (lambda + n) + (1 - alpha.^2 + beta);
for i = 1+1:1:2*n+1
    wm(i) = 1 /(2*(lambda + n));
    wc(i) = 1 /(2*(lambda + n));
end

%R1 and Q1 are covariances of the distribution of added Gaussian noises
R1 = [xErr  0; 0 yErr]; % noise during estimation of the error in the position
Q1 = zErr; % noise during estimation of the error in the measurement

% Get the sigma points of the previous belief
x = mu0(:,ones(1,numel(mu0)));
X0 = [mu0 x+(gamma*sqrtm(sigma0)) x-(gamma*sqrtm(sigma0))];

%---------------------------------------
% State transition prediction
% g is an arbitrary nonlinear function that must represent the real movement as much as possible. 
%X1P = g(u1, X0)
X1P = [X0(1,:)+u1(1) ; X0(2,:)+u1(2)];
%---------------------------------------

%mu1_P = sum[i=0..2n](wm[i]*X1P[i])
mu1_P = zeros(n,1);
for i=1:1:2*n+1
    mu1_P = mu1_P + (wm(i) * X1P(:, i));
end

%sigma1_P = sum[i=0..2n](wc[i](X1P[i] - mu1_P)(X1P[i] - mu1_P)^T + R1
sigma1_P = zeros(n);
for i=1:1:2*n+1
    sigma1_P = sigma1_P + (wc(i) * (X1P(:, i) - mu1_P)*(X1P(:, i) - mu1_P).') ;
end
sigma1_P = sigma1_P + R1;

%Overall uncertainty
x = mu1_P(:,ones(1,numel(mu1_P)));
X1_P = [mu1_P  x+gamma*sqrtm(sigma1_P) x-gamma*sqrtm(sigma1_P)];

%---------------------------------------
% Measurement Prediction function
% h is an arbitrary nonlinear function.
%Z1_P = h(X1_P)
% z = 100 / (sqrt(X1(1)² + X1(2)²))
Z1_P = ((sqrt((X1_P(1,:).^2) + (X1_P(2,:).^2))) * (1/100)).^-1;
%---------------------------------------

% Predicted observation
%z1v = sum[i=0..2n](wm[i]*Z1_P[i])
z1v = zeros(1, 1);
for i=1:1:2*n+1
    z1v = z1v + (wm(i) * Z1_P(: , i));
end

% Uncertainty of the predicted observation
%S1 = sum[i=0..2n](wc[i]*(Z1_P[i] - z1v)(Z1_P[i] - z1v)^T + Q1
S1 = 0;
for i=1:1:2*n+1
    S1 = S1 + (wc(i) * (Z1_P(:, i) - z1v)*((Z1_P(:, i) - z1v).'));
end
S1 = S1 + Q1;

%Cross covariance
%sigma1_Pxz = sum[i=0..2n](wc[i]*(X1_P[i] - mu1_P)(Z1_P[i] - z1v)^T
sigma1_Pxz = 0;
for i=1:1:2*n+1
    sigma1_Pxz = sigma1_Pxz + (wc(i)*(X1_P(:, i) - mu1_P)*(Z1_P(:, i) - z1v).');
end

%Kalman gain
K1 = sigma1_Pxz * (S1^-1);

%mu1 = mu1_P + K1(z1 - z1v)
mu1 = mu1_P + (K1*(z1 - z1v));

%sigma1 = sigma1_P - K1*S1*K1^T
sigma1 = sigma1_P - (K1 * S1 * (K1.'));
end