% Filtre Kalman 2D pour gyroscope et compas magnétique
% Cours GLO-4001/GLO-7021
% (c) 2010-2012 Philippe Giguere, U. Laval

clearvars
clf
myFontSize = 14;
dT = 0.05;  % Plus je prends des mesures frequentes, plus petite est l'erreur
wrot = 0.2; % frequence rotation du systeme

% Variables inconnues décrivant le système.
SGyro    = 0.2; % degree par seconde
SCompas  = 10;   % degree
GyroBiais = 0.1; %biais du gyroscope
xVrai     = [0 0]';

% Variables connues
Cv = [0 0; 0 (4*SGyro)^2];
Cw = SCompas^2; 
F = [1 dT; 0 0];       % Plant model. With [1 dt; 0 1] i might get unstability.
H = [1 0];             % measurement model, we eliminate the gyro; notice it is different direction (no transpose)
B = [1];               % Modele actionneur

% Initialiser le filtre
P = [SCompas^2 0; 0 (SGyro)^2];
X = xVrai;

XGyro = 0;

% Parametre de la simulation
nStep = 4000;
Ax = zeros(1,nStep);
AEx = zeros(1,nStep);
ACompas = zeros(1,nStep);
AGyro = zeros(1,nStep);
ErreurEx = zeros(1,nStep);
ErreurCompas = zeros(1,nStep);
ATime = zeros(1,nStep);
XGyroInt = zeros(1,nStep);


for iStep = 1:nStep
    % Simulation du systeme
    time = iStep*dT;
  
    % Je vais osciller le capteur combine
    xVrai = 40*[sin(wrot*time) wrot*cos(wrot*time)]';

    % Je simule la reponse des capteurs
    compass = (xVrai(1) + SCompas*randn);
    gyro    =  xVrai(2) + SGyro*randn + GyroBiais;

    % Le vecteur de mesure
    z = compass;
    
    % La commande
    U = [0 gyro]';
    
    % ====== Propagation =========
    X = F*X + B*U;
    P = F*P*F' + Cv;
    
    % ======== Mise-a-jour ========
    K = P*H'/(H*P*H'+Cw);
    X = X + K*(z-H*X);
    P = (eye(2)-K*H)*P;
    
    % Integrer le gyro, pour fin de comparaison
    XGyro    = XGyro + gyro*dT;
    XGyroInt(iStep)= XGyro;
    
    % Cueillette des donnees pour les graphiques
    Ax(iStep)           = xVrai(1);
    AEx(iStep)          = X(1);
    ACompas(iStep)      = z(1);
    AGyro(iStep)        = gyro;
    ErreurEx(iStep)     = (X(1)-xVrai(1));
    ErreurCompas(iStep) = (z(1)-xVrai(1));
    ATime(iStep) = time;
end

MSize = 3;
figure(1)
clf
subplot(3,1,1);
tl = 1; th = nStep;
h(1) = plot(ATime(tl:th),Ax(tl:th),'k','LineWidth',2);
hold on;
h(3) = plot(ATime(tl:th),AEx(tl:th),'go','MarkerFaceColor','g','MarkerSize',MSize);
h(2) = plot(ATime(tl:th),ACompas(tl:th),'r*','MarkerSize',MSize-1);
h(4) = plot(ATime(tl:th),XGyroInt(tl:th),'o','MarkerSize',MSize);
% So it is on top
plot(ATime(tl:th),AEx(tl:th),'go','MarkerFaceColor','g','MarkerSize',MSize);
plot(ATime(tl:th),Ax(tl:th),'k','LineWidth',2);
xlabel('Temps (s)','FontSize',myFontSize);
ylabel('Angle (deg)','FontSize',myFontSize);
legend(h,{'Position reelle' 'Compas' 'Kalman' 'Integrale Gyro'},'Location','NorthWest');
set(gca,'FontSize',myFontSize-2);

subplot(3,1,2);
tl = 1; th = 50;
h(1) = plot(ATime(tl:th),Ax(tl:th),'k','LineWidth',2);
hold on;
h(3) = plot(ATime(tl:th),AEx(tl:th),'go','MarkerFaceColor','g');
h(2) = plot(ATime(tl:th),ACompas(tl:th),'r*');
h(4) = plot(ATime(tl:th),XGyroInt(tl:th),'o','LineWidth',2);
xlabel('Temps (s)','FontSize',myFontSize);
ylabel('Angle (deg)','FontSize',myFontSize);
legend(h,{'Position reelle' 'Compas' 'Kalman' 'Integrale Gyro'},'Location','NorthWest');
set(gca,'FontSize',myFontSize-2);

subplot(3,1,3);
tl = nStep-50; th = nStep;
h(1) = plot(ATime(tl:th),Ax(tl:th),'k','LineWidth',2);
hold on;
h(3) = plot(ATime(tl:th),AEx(tl:th),'go','MarkerFaceColor','g');
h(2) = plot(ATime(tl:th),ACompas(tl:th),'r*');
h(4) = plot(ATime(tl:th),XGyroInt(tl:th),'o','LineWidth',2);
xlabel('Temps (s)','FontSize',myFontSize);
ylabel('Angle (deg)','FontSize',myFontSize);
legend(h,{'Position reelle' 'Compas' 'Kalman' 'Integrale Gyro'},'Location','NorthWest');
set(gca,'FontSize',myFontSize-2);

%print -dpng KalmanCompassGyro.png

figure(2)
subplot(2,1,1);
hist(ErreurCompas,20);
std(ErreurCompas)
xlabel('Erreur Compas (deg)','FontSize',myFontSize);
set(gca,'FontSize',myFontSize-2);
c = axis;

subplot(2,1,2);
hist(ErreurEx,20);
xlim(c(1:2));
std(ErreurEx)
xlabel('Erreur Kalman (deg)','FontSize',myFontSize);
set(gca,'FontSize',myFontSize-2);