%% Init

close all; clc; clear;


%% Parameter

Parameter = 1;

if Parameter==1          %Realistische Parameter
    par.mK=2000;
    par.mG=1000;
    par.l=7;
    par.g=9.81;
    par.mu=6000;
    u=1;
    
elseif Parameter==2     % Unrealistische Parameter
    
    par.mK=0.2;
    par.mG=0.1;
    par.l=1;
    par.g=9.81;
    par.mu=0.5;
    u=0.002;
end

%% Elemtente der Matrizen

par.a22=-par.mu/par.mK;
par.a23=par.mG/par.mK*par.g;
par.a42=par.mu/(par.l*par.mK);
par.a43=-(par.mG+par.mK)/par.mK*par.g/par.l;

par.b2=1/par.mK;
par.b4=-1/(par.mK*par.l);

%% Matrizen der Strecke = Matrizen des Beobachters

A=[0 1 0 0; 0 par.a22 par.a23 0; 0 0 0 1 ; 0 par.a42 par.a43 0];
A_opt=[-0.20 1 0 0; 0 par.a22 par.a23 0; 0 0 0 1 ; 0 par.a42 par.a43 -1.5];
B=[0; par.b2; 0; par.b4];
C=[1 0 0 0];
D=0;


%% Bilden von Systemen und überprüfen von Stabilität, Beobachtabrkeit etc.


sys=ss(A,B,C,D);     % Bildet aus dem Vektoren ein Zustandsraummodell
sys_opt=ss(A_opt,B,C,D);

Beobachtbarkeit_ref = rank(obsv(sys));      % Beobachtbarkeit überprüfen
Beobachtbarkeit_opt = rank(obsv(sys_opt));

damp(A);            % Pole, Dämpfung und Frequenz
damp(A_opt);

%% Rauschen

Q=1;        % Prozessrauschen
R=1;        % Messrauschen


%% Kalman Verstärkung
[kalmf, L_Kal, P_Kal] = kalman(sys_opt,Q,R); 

         
%% Simulation

Schalter = 1;  sim('Kalman_Filter_mager',60); simdata1 = simdata_kal;
Schalter = 2;  sim('Kalman_Filter_mager',60); simdata2 = simdata_kal;

%% Plots

fig=figure;
scrsz = get(0,'ScreenSize');
set(fig,'Position',[scrsz(1) scrsz(2) scrsz(3) scrsz(4)])

subplot(211), 
plot(simdata1.time, simdata1.signals(1).values(:,1),'b','LineWidth',2), hold on,
plot(simdata1.time, simdata1.signals(3).values(:,1),'k','LineWidth',1), grid on,
plot(simdata1.time, simdata1.signals(2).values(:,1),'r','LineWidth',2), grid on, 
plot(simdata2.time, simdata2.signals(2).values(:,1),'g','LineWidth',2), grid on,
title('Weg der Katze', 'FontSize', 16), legend('Tatsächlich', 'Gemessen (Verrauscht)','Beobachtet mit L\_LQR', 'Beobachtet mit L\_Simulink', 'Location','southeast')


subplot(212), 
plot(simdata1.time, simdata1.signals(5).values(:,1),'k','LineWidth',1), hold on, grid on, 
plot(simdata1.time, simdata1.signals(4).values(:,1),'r','LineWidth',2), hold on, 
plot(simdata2.time, simdata2.signals(4).values(:,1),'g','LineWidth',2), grid on, 
title('Differenz des beobachteten und tatsächlichen Winkelwerts', 'FontSize', 16), legend('Tatsächliches Rauschen','Beobachter mit L\_LQR', 'Beobachter mit L\_Simulink', 'Location','northeast')