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:
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.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];
source: http://www.cse.cuhk.edu.hk/~phwl/mt/...ts/cctsang.pdf
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];
Ma matrice de transposition H:
Et enfin mon vecteur de mesure Z:
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];
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:
pour y insérer les données du GPS si je ne me trompe pas.
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];
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')
Partager