% UE_3LQE   Entwurf Kalmanfilter fuer Boeing 707-321 bei v=80m/s
%
% G=(A,B,C,D)           Strecke
% u=[u1 u2]             deterministische Anregung
%   u1: Schub f
%   u2: Höhenruderwinkel eta
% y=[y1 y2]             Ausgangsgrößen
%   y1: Geschwindigkeit u
%   y2: Nickwinkel tau
%
% Gt=(At,Bt,Ct,Dt)      Strecke inkl. Kalman-Filter
% u=[u1 u2 xi theta]    Anregung
%   u1: Schub f
%   u2: Höhenruderwinkel eta
% xi=[xi1 ... xi4]      Prozessrauschen (auf x1...x4)
% theta=[theta1 theta2] Sensorrauschen (auf y1 Geschw., y2 Nickwinkel)
% y=[y1 ... y6]         Ausgangsgrößen
%   y1: Geschwindigkeit u
%   y2: Nickwinkel tau
%   y3: Geschwindigkei verrauscht (y1+theta1)
%   y4: Nickwinkel verrauscht (y2+theta2)
%   y5: Schätzwert Geschwindigkeit
%   y6: Schätzwert Nickwinkel
%
% CAD-Uebung zur Vorlesung Entwurf Robuster Regelungen
% Thema: Kalman Filter

% M. Grobe  10/2003
% Technische Universitaet Braunschweig, IfR

% Strecke (Boeing) und Signale initialisieren
ue_3init;

% Berechnung der Covarianzmatrizen
Xi=cov(xi);
Theta=cov(theta);

% LQE Entwurf Kalmanfilter nach VL
% loesen der Riccati-Gleichung (FARE): A*S + S*A' - S*C'*inv(Theta)*C*S + Xi = 0
S=are(A',C'*inv(Theta)*C,Xi);
% Regler berechnen
H=-S*C'*inv(Theta);

% LQE Berechnung des Kalmanfilters mit MATLAB
H2=-lqe(A,eye(size(A)),C,Xi,Theta);

% Gesamtsystem aus Strecke und Kalman-Filter
At=[A zeros(size(A)); -H*C A+H*C];
Bt=[B eye(size(A)) zeros(size(H)); B zeros(size(A)) -H];
Ct=[C zeros(size(C)); C zeros(size(C)); zeros(size(C)) C];
Dt=zeros(6,8); 
Dt(3:4,7:8)=eye(2);

% Simulation System
Gt=ss2sys(At,Bt,Ct,Dt);
y=lsim(Gt,[u1 u2 xi1 xi2 xi3 xi4 theta1 theta2],t);
y=y';
plot(t, [y(:,1) y(:,3) y(:,5)])
title('Kalman-Filter für Geschwindigkeit u')

pause

plot(t, [y(:,2) y(:,4) y(:,6)])
title('Kalman-Filter für Nickwinkel \tau')
