% Solution proposal Task 13
% Remarks: Modified to identify the entire Kalman filter
% Lecture 02.03.2023

clear all   % Clear memory
clc, clf    % clear screen and figures

% Generate some data matrices Y and U to be used for testing purposes
A=0.9; B=0.5; D=1; E=-1;     % Known Model parameters
n=1; m=1;

N=10000;
U=prbs1(N,10,40);

X=zeros(N,n);
Y=zeros(N,m);

w=randn(N,1)*0.05; 
v=randn(N,1)*0.1;
%w=w*0; v=v*0; % For testing without noise
W=cov(w);
V=cov(v);

x=0;
for k=1:N
    y=D*x+w(k);
    
    X(k,:)=x';
    Y(k,:)=y';


    u=U(k,:)';
    x=A*x+B*u+v(k);
end
ny=size(Y,2);

% Parameters for DSR
L=2; g=1; J=2;

[a,b,d,e,c,f,x0]=dsr(Y,U,L,g,J);

k_dsr=c*inv(f)            % DSR estimated Kalman gain

figure(2)
subplot(211), plot(w), title('Measurements noise')
subplot(212), plot(v), title('process noise')

% using dlqe.m function to calculate Kalman gain matrix
V=v'*v/N;  % same as cov(v). The covariance matrix of v_k
W=w'*w/N;  % Same as cov(w). The covariance matrix of w_k

V=0.0001; W=0.001;
[Km,Xm]=dlqe(A,1,D,V,W)

% Translate estimated model a,b,d,e,k_dsr to canonical form (with d=1) in
% order to compare

[ac,bkc,dc,t]=ss2cf(a,[b k_dsr],d);

% In scaled form
a_dsr=ac; b_dsr=bkc(1); k_dsr=bkc(2); d_dsr=dc;


