Bonjour à tous,

J'essaye d'implémenter un filtre de kalman permettant de faire de la fusion de capteurs provenant d'un IMU et d'une autre source me donnant une position et une vitesse absolue (le GPS par exemple). L'IMU me donne un vecteur accélération selon deux axes x et y qui est déjà projetée dans le plan de référence (la terre) grâce à un algorithme qui fusionne le signal d'un gyromètre et de l'accéléromètre. Le GPS me donne un vecteur position et vitesse selon x et y.

Le GPS et l'IMU me fournissent des données à des fréquences différentes: 100Hz pour l'imu et 1Hz pour le GPS. Ces deux sources sont soumises à un bruit gaussien.

C'est le cas classique du filtrage de kalman mais je n'arrive pas à trouver d'exemples simples sur matlab.

J'ai pour l'instant implémenté un filtre ne tenant en compte que de l'IMU:

Mon vecteur d'état est composé de 6 éléments: (px vx ax py vy ay)'
p = position
v = vitesse
a = accélération

Voila ma matrice Q de covariance de bruit de commande:

Code : Sélectionner tout - Visualiser dans une fenêtre à part
1
2
3
4
5
6
7
qc=0.2^2;
 
q1=[(1/20*qc*dt^5) (1/8*qc*dt^4) (1/6*qc*dt^3);
    (1/8*qc*dt^4) (1/3*qc*dt^3) (1/2*qc*dt^2); 
    (1/6*qc*dt^3) (1/2*qc*dt^2) (qc*dt)];
 
s.Q=[q1 o3 ;o3 q1];
Ma matrice R de covariance de bruit de mesure:

Code : Sélectionner tout - Visualiser dans une fenêtre à part
1
2
3
4
5
6
7
qc=0.8^2;
 
q1=[(1/20*qc*dt^5) (1/8*qc*dt^4) (1/6*qc*dt^3);
    (1/8*qc*dt^4) (1/3*qc*dt^3) (1/2*qc*dt^2); 
    (1/6*qc*dt^3) (1/2*qc*dt^2) (qc*dt)];
 
s.R = [q1 o3 ;o3 q1];
source: http://www.cse.cuhk.edu.hk/~phwl/mt/...ts/cctsang.pdf

Ma matrice de transposition H:

Code : Sélectionner tout - Visualiser dans une fenêtre à part
1
2
 a1=[0 0 0;0 0 0; 0 0 1];
s.H=[a1 o3;o3 a1];
Et enfin mon vecteur de mesure Z:

z=[0 0 Ax1(k) 0 0 Ay1(k)]';

Avec Ax1 et Ay1 les vecteurs contenants les valeurs d'accélération de l'accéléromètre.

Comment insérer à ce filtre les données provenant du GPS ? Il me semble que le vecteur d'état ne change pas. En revanche la matrice H devrait changer tous les 1Hz et devenir:
Code : Sélectionner tout - Visualiser dans une fenêtre à part
1
2
 a1=[1 0 0;0 1 0; 0 0 1];
s.H=[a1 o3;o3 a1];
pour y insérer les données du GPS si je ne me trompe pas.

Mais il faut prendre en compte le fait que l'écart type du GPS est différent de celui de l'IMU.

Les données du GPS sont plus fiables et devrait avoir plus de poids lors de la fusion. Comment le prendre en compte ?

Et enfin, sur le net je vois que la majorité des filtre utilisés pour ce type de problématique sont des filtres étendus de kalman. Je ne comprend pas l’intérêt étant donné l'équation du système est linéaire.

Comment feriez vous pour implémenter ce type de filtre ?

Merci pour votre aide.

Voici le code matlab complet:
Code : Sélectionner tout - Visualiser dans une fenêtre à part
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
clear all;
 
% Sampling frequency 
Fs=100;
dt=1/Fs;
 
% Load Acc data 
[Ax,Ay,Az] = readfromexcel('C:\Users\ssuignar\Documents\Matlab\deplacement1D.xlsx','A','B','C');
Ax1= cell2mat(Ax);
Ax1 = Ax1.';
Ay1= cell2mat(Ay);
Ay1 = Ay1.';
 
 Ay1 = (Ay1 ./ 1050) .* 9.8;
 Ax1 = (Ax1 ./ 1050) .* 9.8;
 
o3=zeros(3);
 
% State vector position,velocity,acceleration
s.x=[0 0 0 0 0 0]';
 
a=[1 dt (1/2*dt^2);
   0 1 dt; 
   0 0 1];
 
s.A=[a o3; o3 a];
 
% matrice de covariance du bruit de commande
qc=0.2^2;
 
q1=[(1/20*qc*dt^5) (1/8*qc*dt^4) (1/6*qc*dt^3);
    (1/8*qc*dt^4) (1/3*qc*dt^3) (1/2*qc*dt^2); 
    (1/6*qc*dt^3) (1/2*qc*dt^2) (qc*dt)];
 
s.Q=[q1 o3 ;o3 q1];
 
%matrice de covariance du bruit de mesure
qc=0.8^2;
 
q1=[(1/20*qc*dt^5) (1/8*qc*dt^4) (1/6*qc*dt^3);
    (1/8*qc*dt^4) (1/3*qc*dt^3) (1/2*qc*dt^2); 
    (1/6*qc*dt^3) (1/2*qc*dt^2) (qc*dt)];
 
s.R = [q1 o3 ;o3 q1];
 
a1=[0 0 0;0 0 0; 0 0 1];
s.H=[a1 o3;o3 a1];
 
s.B=zeros(6);
s.u=zeros(6,1);
s.P=s.Q
 
 
for k=1:length(Ax1)    
 
s(end).z=[0 0 Ax1(k) 0 0 Ay1(k)]';
s(end+1)=kalmanf(s(end));
end
 
for k=1:length(Ax1)
Ax1_kalman(k) = s(k).x(3);
vx(k) = s(k).x(2);
px(k) = s(k).x(1);
 
Ay1_kalman(k) = s(k).x(6);
vy(k) = s(k).x(5);
py(k) = s(k).x(4);
end
 
 
figure(1);
subplot(2,1,1);
hz = plot(Ax1((1:end-1)),'b');
hold on;
hk = plot(Ax1_kalman(2:end),'r-');
legend([hz hk],'observations','Kalman output',0);
title('Ax')
hold off;
 
subplot(2,1,2);
yz = plot(Ay1((1:end-1)),'b');
hold on;
yk = plot(Ay1_kalman(2:end),'r-');
legend([yz yk],'observations','Kalman output',0);
title('Ay')
hold off;
 
figure(2);
subplot(2,1,1);
hz = plot(vx);
title('Vx')
subplot(2,1,2);
hk =plot(px);
title('Px')
 
 
figure(3);
subplot(2,1,1);
hz = plot(vy);
title('Vy')
subplot(2,1,2);
hk =plot(py);
title('Py')