clc;
clear all;
close all;

% ------------------- initialisierung, vektorerstellung ----------------

dt = .1; % schrittweite
ende = 100;
t = dt:dt:ende;
len=length(t);

% erstelle gyro vektor (winkelv)
gyro(1:200) = ones(1,200)*0;
gyro(201:400) = ones(1,200)*0.1;
gyro(401:600) = ones(1,200)*0;

%gyro(601:800) = ones(1,200)*-0.1;
%gyro(801:1000) = ones(1,200)*0;
gyro(601:800) = ones(1,200)*0;
gyro(801:1000) = ones(1,200)*0;


% erstelle rohen accvektor (winkel)
acc(1)=0;
for i=2:1000
    acc(i)=acc(i-1)+gyro(i);
end

% füge acc rauschen hinzu
standardabweichung_acc = 1;     % messfehler
varianz_acc = standardabweichung_acc^2;
acc_r = acc + randn(1,len).*standardabweichung_acc;

% füge gyro drift hinzu
gdrift = 0.01;
standardabweichung_gyrodrift = 0;     % rauschen
varianz_gyrodrift = standardabweichung_gyrodrift^2;
gyro_d = gyro + gdrift + randn(1,len).*standardabweichung_gyrodrift;

figure(1); hold on;
plot(t,gyro_d);

% berechne winkel (mit drift) aus gyro integration
winkel_d(1)=0;
for i=2:len
    winkel_d(i) = winkel_d(i-1) + gyro_d(i);
end



% ----------------------- kalman ----------------------------
x = [0;0];            % Zustandsvektor [w;d] (winkel;gyro_drift)

P = [5 0;...          % (initiale) Vorhersageunsicherheit 
     0 5];            % je größer desto mehr wird messwerten vertraut
          
A = [1 -dt;...        % Systemmatrix
     0  1];
 
B = [dt;0];           % Eingangsmatrix
     
C = [1 0];            % Messmatrix 
           
R = [varianz_acc];    % Messrausch Kovarianzmatrix
                      % R = E(z z') 
                      % z -> messrauschen aus y = Cx + z
                      % je kleiner desto mehr wird messwerten
                      % vertraut

q11 = (dt*standardabweichung_acc)^2;
%q11 = 1;
q12 = 0;
q21 = 0;
q22 = (standardabweichung_gyrodrift*dt)^2;
%q22 = 0;

Q = [q11 q12;...        % Prozessrausch Kovarianz (fehler des modells)
     q21 q22];          % Q = E(w w') 
                        % w -> systemrauschen aus x = Ax + Bu + w

I = eye(length(x));     % Identity matrix

u = gyro_d;             % externe Einflüsse (winkelgeschwindigkeit)

y = acc_r; % verrauschte messwerte (wäre höhe aus barometer)


%% kalman berechnung
for n=1:len
    
    % ------------------------------------------------------
    % ---------------------- PREDICT -----------------------
    
    % a priori Zustandsschätzung aus bisherigem Zustand und der externen 
    % Einflüsse (gyro messungen)
    x = A*x + B*u(n);
    
    % Schätzung der a priori Kovarianzmatrix aus voriger Kovarianzmatrix
    P = A*P*A' + Q;
    %P=A*P*A' + Q - A*P*C'*inv(R)*C*P*A';

    % ------------------------------------------------------
    % ----------------------- UPDATE -----------------------
    
    % Innovation aus Messwertdifferenz zw Messwert (acc) und und 
    % a priori state
    w = y(n)-(C*x);
    
    % Innovationskovarianz
    S = (C*P*C'+R);
    
    % Kalman Gain
    K = P*C'*inv(S);
    %K=A*P*C'*inv(S);
    
    % a posteriori Schätzung des Zustands
    x = x + K*w;
    
    % a posteriori Schätzung der Kovarianzmatrix
    P = (I-(K*C))*P;
    
 

    sx_k(n+1) = x(1); % speichern der zustände
    dx_k(n+1) = x(2); % speichern der zustände

    
end

figure(2); hold on;
plot(t,acc,'Color','green','LineWidth', 2);  % original 
plot(t,acc_r,'.r');                          % verrauschte messung
plot(t,sx_k(1:end-1),'-x','Color','black');  % kalman
plot(t,winkel_d);                            % winkel aus gyro (mit drift)

legend('winkel_{original}','winkel_{accmessung}','winkel_{kalman}','winkel_{drift}'); 
title('Figure 2 - winkel');

figure(1);
plot(t,dx_k(1:end-1),'Color','black');  % kalman
legend('gyro_{mit offset}','gyrodrift_{kalman}'); 
title('Figure 1 - winkelgeschwindigkeit');

