% This script is a quick hack of the demo 2 in the SMI 1.0 toolbox.
% All user-interaction has been stripped to have this file
% automatically generate the SMIDemo2_* figures.
% N.H. Bergboer
%
% $Id: Exercise1.m,v 1.1 2012/08/21 16:49:36 callmer Exp $
clear;close all;
% Welcome to demo 2 of the SMI toolbox.  In this demo we 
% will present an example of the use  of the SMI toolbox 
% with multiple data sets.

% First we define our system. This is a three degree of
% freedom mass-spring-damper system (see manual).
% The output y(k) is perturbed by a process noise w(k), and a
% measurement noise v(k)

rho = input('Provide a value for the disturbance rho '); % 1.005;
A = [[.9856 .1628;-.1628 .9856] zeros(2,4);
    zeros(2,2) [.8976 .4305;-.4305 .8976] zeros(2,2);
    zeros(2,4) [.8127 .5690;-.5690 .8127]];
A = A*rho;
B = [.0011;.0134;-.0016;-.0072;.0011;.0034];
C = [1.5119 0 2 0 1.5119 0;
     1.3093 0 0 0 -1.3093 0];
D = zeros(2,1);
Q = 10^(-4)*diag([.0242 3.5920 .0534 1.034 .0226 .2279]);
R = 10^(-2)*diag([2.785 2.785]);
%pause % Press any key to continue.
% We have the availability over three experimental data sets for
% identification. The first with a pink input noise u1, the second
% with a multi-sine input u2, and the third with a binary sequence u3.
% A fourth is used for validation

% Seed the random generator
randn('seed',82);
rand('seed',82);

N = 2000;T=(0:N-1)';
[b,a]=butter(4,.8);
u1 = filter(b,a,randn(N,1));
w1 = randn(N,6)*sqrt(Q);v1 = randn(N,2)*sqrt(R);
ynf1 = dltisim(A,B,C,D,u1);y1=ynf1+dltisim(A,eye(6),C,zeros(2,6),w1)+v1;

% initialize KF

Pk = eye(6);
xk = zeros(6,1);
