Filtre de Kalman - programmation d'une centrale inertielle
Bonjour à tous !
je souhaite programmer mon accéléromètre, gyroscope et mon magnétomètre pour obtenir une estimation des angles d'euler. Je souhaiterai programmer un filtre de Kalman linéaire mais voilà je ne comprends pas pourquoi je n'ai pas les résultats attendus
ci joint mon code en Matlab :
Mon script principal :
ma prediction et mise à jour du filtre de kalman :Code:% Main pour les angles Roll et pitch close all; % close all figures clear; % clear all variables clc; % clear the command terminal % Import and plot sensor data load('ExampleData.mat'); % initialisation des parametres Fe = 256; % Frequence d'échantillonnage Te = 1/Fe; % Periode d'échantillonnage var_wGyroX = 0.5; % variance de la vitesse angulaire du gyroscope selon X var_wGyroY = 0.5; % variance de la vitesse angulaire du gyroscope selon Y var_roll = var_wGyroX * Te * Te; % variance de l'angle roll var_pitch = var_wGyroY * Te * Te; % variance de l'angle pitch varBiaisGyroX = 0.3; % variance du biais du gyroscope selon X varBiaisGyroY = 0.3; % variance du biais du gyroscope selon Y var_Roll_Acc = 0.1; % variance de la mesure de l'angle roll par l'accelerometre var_Pitch_Acc = 0.1; % variance de la mesure de l'angle pitch par l'accelerometre roll = 0; pitch = 0; biaisGyroX = 0; biaisGyroY = 0; wx = 0; wy = 0; % Normalisation des donn꦳ des capteurs if(norm(Accelerometer) == 0), return; end % handle NaN Accelerometer = Accelerometer / norm(Accelerometer); % normalise donn꦳ de l'accꭩrom鵲e if(norm(Magnetometer) == 0), return; end % handle NaN Magnetometer = Magnetometer / norm(Magnetometer); % normalise donn꦳ du magn굯m鵲e if(norm(Gyroscope) == 0), return; end % handle NaN Gyroscope = Gyroscope / norm(Gyroscope); % normalise donn꦳ du gyroscope % Chargement des mesures wGyroX = Gyroscope(:,1); % vitesse angulaire selon X issue du gyroscope wGyroY = Gyroscope(:,2); % vitesse angulaire selon Y issue du gyroscope accAccX = Accelerometer(:,1); % acceleration selon X issue de l'accelerometre accAccY = Accelerometer(:,2); % acceleration selon Y issue de l'accelerometre accAccZ = Accelerometer(:,3); % acceleration selon Z issue de l'accelerometre % Traitement des données issues du magnetometre % Calcul de Pitch d'apr鳠l'accelerometre Pitch_Acc = atan2(accAccY,(sqrt(accAccX.*accAccX + accAccZ.*accAccZ)))*(180/pi); % Calcul de Roll d'apr鳠l'accelerometre Roll_Acc = atan2(accAccX,(sqrt(accAccY.*accAccY + accAccZ.*accAccZ)))*(180/pi); figure('Name', 'Angle acc'); hold on; plot(time, Roll_Acc', 'r'); plot(time, Pitch_Acc', 'g'); title('Angle Acc'); xlabel('Time (s)'); ylabel('Angle rool and pitch (deg)'); legend('pﵣh', 'roll'); hold off; % initialisation des matrices A_KF1 = [1 0 Te 0 0 -Te; % Matrice reliant l'état Xk+1 a Xkcorrespond aussi à F 0 1 0 Te 0 -Te; % correspond aussi à F 0 0 1 0 0 0; 0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1]; B_KF1 =[0 0 0 0 0 0; % pour lier Xk à la commande U 0 0 0 0 0 0; % Xk+1 = AXk+Buk 0 0 0 0 0 0; 0 0 0 0 0 0; 0 0 0 0 0 0; 0 0 0 0 0 0]; R_KF1 = [var_Roll_Acc 0 0 0; % Matrice de covariance de mesure 0 var_Pitch_Acc 0 0; 0 0 var_wGyroX 0; 0 0 0 var_wGyroY]; Q_KF1 = [var_roll 0 0 0 0 0; % Matrice de covariance initiale Q 0 var_pitch 0 0 0 0; 0 0 var_wGyroX 0 0 0; 0 0 0 var_wGyroY 0 0; 0 0 0 0 varBiaisGyroX 0; 0 0 0 0 0 varBiaisGyroY]; P_KF1 = [ 0.5 0 0 0 0 0; % Matrice d'estimation ࡰriori de la covariance de l'erreur initiqlisꥠavec une grande erreur 0 0.5 0 0 0 0; 0 0 0.5 0 0 0; 0 0 0 0.5 0 0; 0 0 0 0 0.5 0 0 0 0 0 0 0.5]; H_KF1 = [ 1 0 0 0 0 0; % Matrice reliant la msure au vecteur d'굡t 0 1 0 0 0 0; 0 0 1 0 1 0; 0 0 0 1 0 1]; x_KF1 = [roll ; % Vecteur d'état pitch ; wx; wy; biaisGyroX ; biaisGyroY ]; % Application du filtre for i=1:length(time) u_KF1 = [0 ; % pour lier Xk à la commande U 0 ; 0 ; 0 ; 0 ; 0 ] ; y_KF1 = [ Roll_Acc(i) ; Pitch_Acc(i); wGyroX(i); wGyroY(i)] % Vecteur de mesure issu de l'accelerometre % utilisation de la fonction pour l'étape de prédiction [x_KF1,P_KF1] = kf_predict(x_KF1,P_KF1,A_KF1,Q_KF1,B_KF1,u_KF1); % utilisation de la fonction pour l'étape de mise a jour [x_KF1,P_KF1,K_KF1,IM_KF1,IS_KF1,LH_KF1] = kf_update(x_KF1,P_KF1,y_KF1,H_KF1,R_KF1); roll_Kalman_Acc(i) = x_KF1(1); pitch_Kalman_Acc(i) = x_KF1(2); wGyroX_Kalman(i) = x_KF1(3); wGyroY_Kalman(i) = x_KF1(4); biaisGyroX_Kalman(i) = x_KF1(5); biaisGyroY_Kalman(i) = x_KF1(6); end % Integration de la vitesse angulaire du gyroscope pour obtenir l'angle for k=1:length(time) if(k==1) roll_Gyro(k) = 0 + Te * (wGyroX(k) - biaisGyroX_Kalman(k)); pitch_Gyro(k) = 0 + Te * (wGyroY(k) - biaisGyroY_Kalman(k)); else roll_Gyro(k) = roll_Gyro(k-1) + Te * (wGyroX(k) - biaisGyroX_Kalman(k)); pitch_Gyro(k) = pitch_Gyro(k-1) + Te * (wGyroY(k) - biaisGyroY_Kalman(k)); end end
Si vous voyez des erreurs, faites le moi savoir je suis débutante... et mon filtre ne fonctionne pas du tout. Je ne comprends pas pourquoi je n'ai pas de résultats linéaire pour mon biais.Code:function [x,P] = kf_predict(x,P,A,Q,B,u) % % Check arguments % if nargin < 3 A = []; end if nargin < 4 Q = []; end if nargin < 5 B = []; end if nargin < 6 u = []; end % % Apply defaults % if isempty(A) A = eye(size(x,1)); end if isempty(Q) Q = zeros(size(x,1)); end if isempty(B) & ~isempty(u) B = eye(size(x,1),size(u,1)); end % % Perform prediction % if isempty(u) x = A * x; P = A * P * A' + Q; else x = A * x + B * u; P = A * P * A' + Q; end function [X,P,K,IM,IS,LH] = kf_update(X,P,y,H,R) % % Check which arguments are there % if nargin < 5 error('Too few arguments'); end % % update step % IM = H*X; % y = mesure - IM / innovation IS = (R + H*P*H'); % = S K = P*H'/IS; % gain du filtre de Kalman X = X + K * (y-IM); % Calcul de l'état mis à jour P = P - (K * H) * P; % Covariance mise à jour if nargout > 5 LH = gauss_pdf(y,IM,IS); end
merci d'avance,
Lucile
-----