0
$\begingroup$

Assume that we have a first order dynamical system

$$G(s) = \frac{1}{0.2s + 2.1}$$

I run this with an input $u = 10sin(t)$ and with gaussian noise.

% Dynamical model
G = tf(1, [0.2 2.1]);

% Input signal
N = 200;
t = linspace(0, 10, N);
u = 10*sin(t);

% Add some noise
e = randn(1, N);

% Simulate the model with input signal
y_disturbance = lsim(G, u+e, t);

enter image description here

Then I want to estimate the disturbance. So I builded a special algorithm called "Stochastic Realization Algorithm". Here I'm using MataveID for the simulation.

This code gives back a discrete dynamical system on the form

$$x(k+1) = Ax(k) + Ke(k)$$ $$y(k) = Cx(k) + e(k)$$

Where $e(k)$ is gaussian noise and $K$ is the kalman gain matrix. It's displayed as $B$-matrix on the original state space form, where $D = 1$ and $u(k)$ is replaced with $e(k)$

$$x(k+1) = Ax(k) + Bu(k)$$ $$y(k) = Cx(k) + Du(k)$$

function [sysd] = sra(y, k, sampleTime)

  % Strucure the hankel matrix of outputs
  [p, Ndat] = size(y);
  N = Ndat-2*k;
  j = 0;
  for i = 1:p:2*k*p-p+1
    j = j+1;
    Y(i:i+p-1,:) = y(:,j:j+N-1);
  end

  % Past outputs and future outputs
  Yp = Y(1:k*p,:);
  Yf = Y(k*p+1:2*k*p,:);

  % LQ decomposition - Better to do 1/sqrt(N) after qr() function
  H = [Yp; Yf];
  [Q,L] = qr(H',0);
  L = L'/sqrt(N); % Eq. (8.76)

  L11 = L(1:k*p,1:k*p);
  L21 = L(k*p+1:2*k*p,1:k*p);
  L22 = L(k*p+1:2*k*p,k*p+1:2*k*p);

  % Covariance matrices
  Rff = (L21*L21'+ L22*L22');
  Rfp = L21*L11';
  Rpp = L11*L11';

  % Equation 8.77: Instead of doing cholesky decomposition Rff = L*L^T and Rpp = M*M^T
  [Uf,Sf,Vf] = svd(Rff);
  [Up,Sp,Vp] = svd(Rpp);
  Sf = sqrtm(Sf);
  Sp = sqrtm(Sp);
  L = Uf*Sf*Vf';
  M = Up*Sp*Vp';

  % Do model reduction
  [U,S,V] = svd(inv(L)*Rfp*inv(M)', 'econ');
  [U, S, V, n] = modelReduction(U, S, V);

  % Find the states
  Xbar = sqrt(S)*V'*inv(M)*Yp;

  % Compute the estimated states and outputs
  Xhat1 = Xbar(1:n, 2:N);
  Xhat = Xbar(1:n, 1:N-1);
  Yhat = Yf(1:p, 1:N-1);

  % Compute A and C
  AC = [Xhat1; Yhat]/[Xhat];
  Ad = AC(1:n, :);
  Cd = AC(n+1:n+p, :);

  % Get noise e and disturbance w
  w = Xhat1 - Ad*Xhat;
  e = Yhat - Cd*Xhat;

  % Computing the covariance matrix
  covariance = [w*w' w*e'; e*w' e*e']/(N-1);

  % Compute Q, R, S for the riccati equation
  Qhat = covariance(1:n, 1:n);
  Rhat = covariance(n+1:n+p, n+1:n+p);
  Shat = covariance(1:n, n+1:n+p);

  % Create a temporary state space model
  delay = 0;
  riccati = ss(delay, Ad', Cd', zeros(p, n), zeros(p, p));
  riccati.sampleTime = sampleTime;

  % Find kalman filter gain matrix K - Use IDARE in MATLAB
  [~, K] = are(riccati, Qhat, Rhat, Shat);

  % Or you could uncomment these instead....
  %Lambda = Rpp(1:p,1:p); % Covariance matrix of output
  %Ok = L*U*sqrtm(S); % Eq. (8.79)
  %Ck = sqrtm(S)*V'*M';
  %Ad = Ok(1:k*p-p,:) \ Ok(p+1:k*p,:);
  %Cd = Ok(1:p,:);
  %Bd = Ck(:,(k-1)*p+1:k*p)';
  %R = Lambda-Cd*S*Cd';
  %K = (Bd'-Ad*S*Cd')/R;

  % Create the model - Use ss in MATLAB
  sysd = ss(delay, Ad, K', Cd, zeros(p, p));
  sysd.sampleTime = sampleTime;
end

function [U1, S1, V1, nx] = modelReduction(U, S, V)
  % Plot singular values
  stem(1:length(S), diag(S));
  title('Hankel Singular values');
  xlabel('Amount of singular values');
  ylabel('Value');

  % Choose system dimension n - Remember that you can use modred.m to reduce some states too!
  nx = inputdlg('Choose the state dimension by looking at hankel singular values: ');
  nx = str2num(cell2mat(nx));

  % Choose the dimension nx
  U1 = U(:, 1:nx);
  S1 = S(1:nx, 1:nx);
  V1 = V(:, 1:nx);
end

If I want to find the disturbance, then I need to extract

% Extract the disturbance
Ghat = G; % We assumed that we have identify the model G
y_clean = lsim(Ghat, u, t);
close
yd = y_disturbance - y_clean;
plot(t, yd)

enter image description here

So now I want to create ARMA disturbance model from yd.

% Find ARMA model
k = 20; % Tuning parameter
sampleTime = t(2) - t(1);
ARMA = sra(yd, k, sampleTime);

I select hankel singular values as 2, so I get second order ARMA model

enter image description here

Then I want to simulate my ARMA model with noise

% Simulate the ARMA model with input signal e
lsim(ARMA, e, t);

enter image description here

If I try to simulate the "estimated" transfer function with the noise.

% Try to simulate only with Ghat and e as input
lsim(Ghat, e, t);

enter image description here

Question:

Have I estimate a correct ARMA model? Or is it wrong estimated? My goal is to find the disturbance, but I don't know how.

$\endgroup$

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.