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
97
98
99
100
101
102
103
104
105
106
107
108
109
|
% KALMAN simulation file
close all;
clear all;
%%%%%%%%%%%%%%%%%%%%%%%%
% constant declaration %
%%%%%%%%%%%%%%%%%%%%%%%%
c=299792458; % speed of light in m/s
delta_t=0.0001; % trajectory sampling step
t=0:delta_t:1; % time vector declaration
n=length(t); % total number of time samples
Vx=200; % velocity of the mobile along the x axis in m/s (line traj.)
Vy=20; % velocity of the mobile along the y axis in m/s (line traj.)
Ax=1; % acceleration of the mobile along the x axis in m/s (line traj.)
Ay=1; % acceleration of the mobile along the x axis in m/s (line traj.)
X0=0;
Y0=0;
f=15*10^9;% fréquence
c=3*10^8;%celerité
lambda=c/f;%longueur d'onde% matrix containing the transmitters position
E=[0 20000;
0 -20000];
sigma=[10;10]; % mesure noise std for each transmitter
N=length(E); % number of transmitters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% generation of the true trajectory %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:n
x(i)=X0+Vx*t(i)+1/2*Ax*t(i)^2; % x-coordinate of the mobile in meters
y(i)=Y0+Vy*t(i)+1/2*Ay*t(i)^2; % y-coordinate of the mobile in meters
% x(i)=Rx*cos(2*pi*Fx*t(i)); % x-coordinate of the mobile in meters
% y(i)=Ry*sin(2*pi*Fy*t(i)); % y-coordinate of the mobile in meters
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% generation of the measurements %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:n
% current time = t(i)
for j=1:N
% current transmitter = transmitter j
distance_true = sqrt((E(j,1)-x(i))^2+(E(j,2)-y(i))^2);
noise = sigma(j)*randn(1,1);
Y(i,j) = distance_true + noise;
end;
end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% determination of the position + clock bias through EKF %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
R=diag(sigma.^2);
sig_ax = 0.05; % std velocity along x
sig_ay = 0.05; % std velocity along y
Q= zeros(6,6);
Q(1,1) = sig_ax^2*delta_t; Q(4,4) = sig_ay^2*delta_t;
F=[1 delta_t 0 0 0 0 ; 0 1 delta_t 0 0 0 ; 0 0 1 0 0 0 ; ...
0 0 0 1 delta_t 0 ; 0 0 0 0 1 delta_t ; 0 0 0 0 0 1 ]; % state transition matrix
S=diag([1e2 1e2 1e2 1e2 1e2 1e2]); % initialisation of the mat cov error state estimation
X_kf=zeros(n,6); % declaration of variable to contain successive estimations
DY = zeros(n,length(sigma));
X_kf(1,:)=[0 0 0 0 0 0]; % initialisation of state estimation
for i=1:n
% 1st KF equation: prediction a priori of the state
if i==1
X_hat=F*X_kf(1,:)';
else
X_hat=F*X_kf(i-1,:)';
end;
% 2nd KF equation: mat cov error estimation a priori
S=F*S*F'+Q;
% calculation of measurement prediction & derivative observation matrix at point X_hat
H = zeros(N,6);
for j=1:N
distance_pred(j)=sqrt((E(j,1)-X_hat(1))^2+(E(j,2)-X_hat(4))^2);
mes_pred(j) = distance_pred(j);
H(j,1)=(X_hat(1)-E(j,1))/distance_pred(j);
H(j,4)=(X_hat(4)-E(j,2))/distance_pred(j);
end;
%Kalman
K=S*H'*inv(H*S*H'+R);
DY(i,:)=Y(i,:)-mes_pred;
X_kf(i,:)=(X_hat+K*DY(i,:)')';
S=S-K*H*S;
X_hat=X_kf(i,:)';
end;
% EKF only
figure(1); hold on; grid on;
plot(x, y, '*-k');
plot(X_kf(:,1), X_kf(:,4),'o-g');
xlabel('X (m)'); ylabel('Y (m)'); title('Comparison True / Estimated Trajectories');
legend('True','KALMAN'); |
