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);

  % 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;

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');

  % 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);

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);
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


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.



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.