% UE_3INIT   Initialisierung der Strecke (Boeing 707-321 bei v=80m/s) 
%            / Entwurf Kalmanfilter
% 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
%
% CAD-Uebung zur Vorlesung Entwurf Robuster Regelungen
% Thema: Kalman Filter

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

% Strecke (Boeing)
A = [ -0.46E-01                0.10681415316     0.0    -0.17121680433    
      -0.1675901504661613     -0.515             1.0     0.6420630320636088E-02
       0.1543104215347786     -0.547945         -0.906  -0.1521689385990753E-02
       0.0                     0.0               1.0     0.0 ];
B = [ 0.1602300107479095       0.2111848453E-02
      0.8196877780963616E-02  -0.3025E-01
      0.9173594317692437E-01  -0.75283075   
      0.0                      0.0 ];
C = [ 1.0                      0.0               0.0    0.0
      0.0                      0.0               0.0    1.0 ];
D = zeros(2);
G=ss(A,B,C,D);

% Rauschen
randn('seed',0);
t=[linspace(0,50,500)]';
% deterministische Anregung u
u1=sin(t);  
u2=cos(t);
u=[u1 u2];
f=u1;
eta=u2;
% Prozessrauschen xi
xi1=sqrt(0.5)* randn(size(t)); 
xi2=sqrt(1)* randn(size(t));
xi3=sqrt(1)* randn(size(t));   
xi4=sqrt(0.5)* randn(size(t));
xi=[xi1 xi2 xi3 xi4];
% Sensorrauschen theta
theta1=sqrt(3)* randn(size(t));   
theta2=sqrt(2)* randn(size(t));
theta=[theta1 theta2];

[nA,nB]=size(B);
[nC,nA]=size(C);

clc
disp('Das System der Boeing 707-321 bei v=80 m/s')
disp('ist in den Matrizen A,B,C,D abgelegt.')
disp(' ')
disp('Dimension des Systems:')
fprintf('  Zustandsgroessen: %i\n',nA)
fprintf('  Eingangsgroessen: %i\n',nB)
fprintf('  Ausgangsgroessen: %i\n',nC)
disp('Anregungen:')
disp('  deterministische Anregungen: u1, u2')
disp('  stochastische Rauschsignale: xi1, xi2, xi3, xi4 (Prozessrauschen)')
disp('                               theta1, theta2 (Sensorrauschen)')
