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) xgamma*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
