% Example as exercise related to Exercise6.
% Demo of Kalman filter for SISO one state system

clear all

A=0.9; B=1; D=1     % Model parameters
V=0.5^2; W=0.1^2;   % Noise covariance matrices

N=10000;
w=randn(N,1)*0.1;   % White measurements noise process
v=randn(N,1)*0.5;   % White process noise process

U=ones(N,1);        % Define some inputs, e.g. a unit step change
U=prbs1(N,20,90);
Y=zeros(N,1);       % Allocate space/vector for the outputs
X=Y;
Xb_a=zeros(N,1);    % Arrays to store the state estimates
Xh_a=zeros(N,1);
K_a=zeros(N,1);

% Initial values for apriori state estimate, xb and covariance matrix 
% Xb
xb=0;
Xb=1000;

x=10;
for k=1:N             % Main simulation loop
    % System output
    y=D*x + w(k);
    
    % The Kalman filter
    yb=D*xb;     % Calculate predicted output
    K=Xb*D'*inv(D*Xb*D'+W);  % K=Xb*D/(Xb*D^2+W); 
    K_a(k)=K;              % Save Kalman gains
    ep=y - yb;             % The innovations process
    xh = xb + K*ep;        % The aposteriori state estimate
    Xb_a(k)=xb;            % Storing apriori estimates
    Xh_a(k)=xh;            % Storing aposteriori state estimates
    
    xb=A*xh + B*U(k);      % Update apriori state estimate
    
    Xh =(1-K*D)*Xb*(1-K*D)' + K*W*K';
    Xb = A*Xh*A'+V;
    % End Kalman filter eqs'..
    
    % Save variables
    Y(k)=y;
    X(k)=x;      
    
    
    % Define input
    u=U(k);
    
    x=A*x+B*u+v(k);  % May change to non-linear model, x = x + dt*f(x,u,v)
    

end

% Plotting
figure(1)
subplot(311), plot(U), title('The input u_k')
subplot(312), plot(Y), title('The output y_k')
subplot(313), plot(X), title('The state x_k')
xlabel('Samples 1 <= k <= N')

figure(2)
t=1:N     % Time
plot(t,X,'b-',t,Xh_a,'r-',t,Xb_a,'k-'), grid
legend('True states x_k','The aposteriori est.', 'apriori  est.')

figure(3)
dXh=X-Xh_a;
plot(dXh), grid
title('Error between state and aposteriori state estimate')

% Varianse
dXh'*dXh/N    % Expected to be close to Xh

% Using DSR to identify Kalman filter on innovations form. 
[a,b,d,e,c,f,x0]=dsr(Y,U,3,0,5,1,1);
k=c*inv(f); d*k, D*A*K
% Above D*A*K and A*K gain in innovations formulation is equal to d*k from DSR
% remember to scale to model with D=1 or check the impulse response
% matrices which are unique

% EPILOG
% For linear systems we could run the above example with a constant
% Kalman filter gain matrix K. This kalman filter gain matrix K could have
% been calculated in advance, i.e. using the dlqe.m function as
% K=dlqe(A,1,D,V,W) or using the kalman equations in a foor-loop.
