主要参考：《基于子空间辨识的船舶微电网模型预测控制策略研究》- 金慧敏

# 一、MOESP 算法概述

  MOESP (Multivariable Output Error State Space, 多变量输出误差状态空间) 方法是系统辨识领域中一种常用的子空间方法，其于 1993 年由
Verhaegen 提出。MOESP 算法的辨识过程包含两个步骤。首先是通过给定的 I/O 数据构造 Hankel 矩阵，对其进行实施 QR 分解技术，然后由分解得到的子空间对扩展观测矩阵进行估计。其次是通过估计扩展的观测矩阵来获取辨识对象的状态空间模型的系数。虽然 MOESP 算法在求解输入矩阵 B 和反馈矩阵 D 时需要构造一个较大的矩阵，但与使用最小二乘求解系统矩阵和输出矩阵的 N4SID 算法的过程相比，该方法求解过程更加简洁、方便，其准确性也同样可以得到保证。

# 二、MOESP 算法主要流程

# 2.1 数据预处理

  考虑一个$m$ 输入，$p$ 输出的离散线性时不变系统：

$$\begin{aligned} \mathbf{x}(t+1) & =\mathbf{A} \mathbf{x}(t)+\mathbf{B} \mathbf{u}(t) \\ \mathbf{y}(t) & =\mathbf{C} \mathbf{x}(t)+\mathbf{D} \mathbf{u}(t) \quad t=0,1, \cdots \end{aligned}$$

  假设$(\mathbf{A}, \mathbf{B})$ 是可达的且$(\mathbf{C}, \mathbf{A})$ 是可观的，简单的定义上式所表达的 LTI 系统描述为$\mathbf{\Sigma} =(\mathbf{A}, \mathbf{B}, \mathbf{C}, \mathbf{D})$。通过求解上式，可以得到：

$$\mathbf{y}(t)=\mathbf{C} \mathbf{A}^t \mathbf{x}(0)+\mathbf{D} \mathbf{u}(t)+\sum_{i=0}^{t-1} \mathbf{C} \mathbf{A}^{t-1-i} \mathbf{B} \mathbf{u}(i), \quad t=0,1, \cdots$$

  如果$\mathbf{u}(t)=0,t=0,1,\cdots$，上式可以化简为：

$$\mathbf{y}(t)=\mathbf{C} \mathbf{A}^t \mathbf{x}(0), \quad t=0,1, \cdots$$

  上式称为零输入响应。此外，如果$\mathbf{x}(0)=0$，则有：

$$\mathbf{y}(t)=\mathbf{D} \mathbf{u}(t)+\sum_{i=0}^{t-1} \mathbf{C} \mathbf{A}^{t-1-i} \mathbf{B} \mathbf{u}(i), \quad t=0,1, \cdots$$

  上式表示由外部输入$\mathbf{u}(t)$ 引起的响应，这种称为零状态响应。因此，线性状态空间方程的响应可以表示为零输入响应和零状态响应两者之和。

对于零状态响应，定义$p\times m$ 维矩阵：

$$\mathbf{G}_t=\left\{\begin{aligned} \mathbf{D},\quad & \mathrm{t}=0 \\ \mathbf{C} \mathbf{A}^{t-1} \mathbf{B},\quad & \mathrm{t}=1,2, \cdots \end{aligned}\right.$$

  其中$(\mathbf{G}_0, \mathbf{G}_1, \cdots)$ 被称作 LTI 系统的脉冲响应，或者马尔可夫参数。

  MOESP 算法要解决的问题是：假设给定输入输出数据${\mathbf{u}(t), \mathbf{y}(t),t=0,1,\cdots,N-1}$，问题是确定系统维数$n$ 和系统矩阵$(\mathbf{A},\mathbf{B},\mathbf{C},\mathbf{D})$。这正是确定性 LTI 系统的子空间辨识问题。

  假设给定输入输出数据${\mathbf{u}(t), \mathbf{y}(t),t=0,1,\cdots,N-1}$，其中$N$ 足够大，然后对于$k>0$ 有：

$$\begin{aligned} &\begin{aligned} & {\left[\begin{array}{llll} \mathbf{y}(0) & \mathbf{y}(1) & \cdots & \mathbf{y}(N-1) \end{array}\right]} \\ & =\left[\begin{array}{lll} \mathbf{G}_{k-1} & \cdots & \mathbf{G}_0 \end{array}\right]\left[\begin{array}{cccccc} 0 & \cdots & 0 & \mathbf{u}(0) & \cdots & \mathbf{u}(N-k) \\ \vdots & \ddots & \ddots & \mathbf{u}(1) & \cdots & \mathbf{u}(N-k+1) \\ 0 & \ddots & \ddots & \vdots & & \vdots \\ \mathbf{u}(0) & \mathbf{u}(1) & \cdots & \mathbf{u}(k-1) & \cdots & \mathbf{u}(N-1) \end{array}\right] \end{aligned} \end{aligned}$$

  假设右侧由输入构成的宽矩阵是满秩的，然后通过最小二乘法求解上述方程可以得到脉冲响应$(\mathbf{G}_{k-1}, \cdots, \mathbf{G}_0)$。这表明在一定的假设下，可以通过使用输入输出数据，而不适用脉冲响应来计算 LTI 系统的最小实现。

# 2.2 构造 Hankel 矩阵

  假设给定输入和输出：

$$\begin{pmatrix} \mathbf{u}\left ( 0 \right ) & \mathbf{u}\left ( 1 \right ) & \cdots & \mathbf{u}\left ( k+N-2 \right ) \end{pmatrix}$$

$$\begin{pmatrix} \mathbf{y}\left ( 0 \right ) & \mathbf{y}\left ( 1 \right ) & \cdots & \mathbf{y}\left ( k+N-2 \right ) \end{pmatrix}$$

  其中$k$ 严格大于状态向量的维数$n$，$N$ 为 Hankel 矩阵的列数。由这些数据，形成块 Hankel 矩阵：

$$\mathbf{U}_{0|k-1}=\begin{bmatrix} \mathbf{u}(0) & \mathbf{u}(1) & \cdots & \mathbf{u}(N-1)\\ \mathbf{u}(1) & \mathbf{u}(2) & \cdots & \mathbf{u}(N)\\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{u}(k-1) & \mathbf{u}(k) & \cdots & \mathbf{u}(k+N-2) \end{bmatrix} \in \mathbb{R}^{km\times N}$$

$$\mathbf{Y}_{0|k-1}=\begin{bmatrix} \mathbf{y}(0) & \mathbf{y}(1) & \cdots & \mathbf{y}(N-1)\\ \mathbf{y}(1) & \mathbf{y}(2) & \cdots & \mathbf{y}(N)\\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{y}(k-1) & \mathbf{y}(k) & \cdots & \mathbf{y}(k+N-2) \end{bmatrix} \in \mathbb{R}^{kp\times N}$$

  其中索引$0$ 和$k-1$ 分别表示第一列的首元素和末元素下标。块 Hankel 矩阵的列数通常确定为$N$，且$N$ 足够大。

  原始系统的 Toeplitz 矩阵为：

$$\mathbf{\psi}_k=\left[\begin{array}{cccc} \mathbf{D} & \mathbf{0} & \cdots & \mathbf{0} \\ \mathbf{C B} & \mathbf{D} & \cdots & \mathbf{0} \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{C A}^{k-2} \mathbf{B} & \mathbf{C A}^{k-3} \mathbf{B} & \cdots & \mathbf{D} \end{array}\right]$$

  扩展可观性矩阵为：

$$\mathbf{\Gamma} _k=\begin{bmatrix} \mathbf{C} \\ \mathbf{CA} \\ \vdots \\ \mathbf{CA^{k-1}} \end{bmatrix}$$

  由此，可以得到如下关系式：

$$\mathbf{Y}_{0|k-1}=\mathbf{\Gamma} _k\mathbf{X}_0+\mathbf{\psi}_k\mathbf{U}_{0|k-1}$$

  其中$\mathbf{X}_0=\begin{bmatrix} x(0) & x(1) & \cdots x(N-1) & \end{bmatrix}\in \mathbb{R}^{n\times N}$ 为初始状态矩阵。

# 2.3 假设

（1）$\text{rank}(\mathbf{X}_0)=n$；

（2）$\text{rank}(\mathbf{U}_{0|k-1})=km$，其中$k>n$；

（3）$\text{span}(\mathbf{X}_0)\cap \text{span}(\mathbf{U}_{0|k-1})=\left \{ 0 \right \}$；

  其中假设（1）的含义是状态向量是充分激励的，或者系统是可达的。假设（2）表明输入序列应满足$k$ 阶的持续激励（PE）条件。如果以上假设以及$\text{rank}(\mathbf{\Gamma} _k)=n$ 都满足，那么下列秩条件成立：

$$\text{rank}\begin{bmatrix} \mathbf{U}_{0|k-1} \\ \mathbf{Y}_{0|k-1} \end{bmatrix}=km+n$$

# 2.4 LQ 分解

  通过对 Hankel 矩阵进行 LQ 分解可以得到：

$$\begin{bmatrix} \mathbf{U}_{0|k-1} \\ \mathbf{Y}_{0|k-1} \end{bmatrix}=\begin{bmatrix} \mathbf{L}_{11} & \mathbf{0}\\ \mathbf{L}_{21} & \mathbf{L}_{22} \end{bmatrix}\begin{bmatrix} \mathbf{Q}_{1}^{\mathrm{T}} \\ \mathbf{Q}_{2}^{\mathrm{T}} \end{bmatrix}$$

  LQ 分解的实际计算是通过对矩阵的 QR 分解进行转置来完成的。在假设 2.3 下，$L$ 矩阵的每一列是一个输入输出对，尤其$L_{22}$ 的每一列都包含系统的一个零输入响应，此外有$\text{rank}(\mathbf{L}_{22})=n$，即系统的维数。由上式可知：

$$\mathbf{U}_{0|k-1}=\mathbf{L}_{11}\mathbf{Q}_{1}^{\mathrm{T}}$$

$$\mathbf{Y}_{0|k-1}=\mathbf{L}_{21}\mathbf{Q}_{1}^{\mathrm{T}}+\mathbf{L}_{22}\mathbf{Q}_{2}^{\mathrm{T}}$$

  易得$\mathbf{L}_{11}$ 是非奇异的，所以可以得到$\mathbf{Q}_{1}^{\mathrm{T}}=\mathbf{L}_{11}^{-1}\mathbf{U}_{0|k-1}$，因此可以得到：

$$\mathbf{Y}_{0|k-1}=\mathbf{L}_{21}\mathbf{L}_{11}^{-1}\mathbf{U}_{0|k-1}+\mathbf{L}_{22}\mathbf{Q}_{2}^{\mathrm{T}}$$

  由于$\mathbf{Q}_{1}$ 和$\mathbf{Q}_{2}$ 是相互正交的矩阵，上式右边的首项是由$\mathbf{U}_{0|k-1}$ 中的行向量组成的，第二项和它正交。因此，$\mathbf{Y}_{0|k-1}$ 的行空间在$\mathbf{U}_{0|k-1}$ 上的正交投影为：

$$\hat{E}\left \{ \mathbf{Y}_{0|k-1} | \mathbf{U}_{0|k-1} \right \} = \mathbf{L}_{21}\mathbf{Q}_{1}^{\mathrm{T}}=\mathbf{L}_{21}\mathbf{L}_{11}^{-1}\mathbf{U}_{0|k-1}$$

  向量$\mathbf{Y}_{0|k-1}$ 的行空间在$\mathbf{U}_{0|k-1}$ 的行空间的补集$\mathbf{U}_{0|k-1}^{\bot}$ 上的正交投影为：

$$\hat{E}\left \{ \mathbf{Y}_{0|k-1} | \mathbf{U}_{0|k-1}^{\bot} \right \} = \mathbf{L}_{22}\mathbf{Q}_{2}^{\mathrm{T}}$$

  由于$\mathbf{Y}_{0|k-1}=\mathbf{\Gamma} _k\mathbf{X}_0+\mathbf{\psi}_k\mathbf{U}_{0|k-1}$，可以得到：

$$\mathbf{\Gamma} _k\mathbf{X}_0+\mathbf{\psi}_k\mathbf{L}_{11}\mathbf{Q}_{1}^{\mathrm{T}}=\mathbf{L}_{21}\mathbf{Q}_{1}^{\mathrm{T}}+\mathbf{L}_{22}\mathbf{Q}_{2}^{\mathrm{T}}$$

  值得注意的是，虽然右边是一个正交和，但左边是一个直接和，因此左边的两个量不一定是正交的，这表明$\mathbf{\Gamma} _k\mathbf{X}_0\ne \mathbf{L}_{22}\mathbf{Q}_{2}^{\mathrm{T}}$，$\mathbf{\psi}_k\mathbf{L}_{11}\mathbf{Q}_{1}^{\mathrm{T}}\ne \mathbf{L}_{21}\mathbf{Q}_{1}^{\mathrm{T}}$。将上式右乘$\mathbf{Q}_{2}^{\mathrm{T}}$ 得到：

$$\mathbf{\Gamma} _k\mathbf{X}_0\mathbf{Q}_{2}=\mathbf{L}_{22}$$

  在$\text{rank}\begin{bmatrix} \mathbf{U}_{0|k-1} \\ \mathbf{Y}_{0|k-1} \end{bmatrix}=km+n$的假设下，可知$\mathbf{X}_0\mathbf{Q}_{2}$ 行满秩并且$\text{rank}(\mathbf{\Gamma}_{k})=n$，等于$\text{rank}(\mathbf{L}_{22})\in \mathbb{R}^{kp\times kp}$。因此，由 SVD 分解$\mathbf{L}_{22}$ 得到维数$n$。

# 2.5 SVD 分解

  将$\mathbf{L}_{22}\in \mathbb{R}^{kp\times kp}$ 进行 SVD 得到：

$$\mathbf{L}_{22}=\begin{bmatrix} \mathbf{U}_1 & \mathbf{U}_2 \end{bmatrix}\begin{bmatrix} \mathbf{\Sigma}_1 & \mathbf{0}\\ \mathbf{0} & \mathbf{0} \end{bmatrix}\begin{bmatrix} \mathbf{V}_1^{\mathrm{T}} \\ \mathbf{V}_2^{\mathrm{T}} \end{bmatrix}=\mathbf{U}_1 \mathbf{\Sigma}_1 \mathbf{V}_1^{\mathrm{T}}$$

  从而有：

$$\mathbf{\Gamma} _k\mathbf{X}_0\mathbf{Q}_{2}=\mathbf{U}_1 \mathbf{\Sigma}_1 \mathbf{V}_1^{\mathrm{T}}$$

  从而有：

$$\mathbf{\Gamma} _k=\mathbf{U}_1 \mathbf{\Sigma}_1^{1/2}\mathbf{T}$$

$$\mathbf{X}_0=\mathbf{T}^{-1}\mathbf{\Sigma}_1^{1/2} \mathbf{V}_1^{\mathrm{T}}\mathbf{Q}_{2}^{-1}$$

其余的步骤与 N4SID 方法一致。

# 三、参考 matlab 程序

% --------------------------------------------------------

% Author: Alexander Robles

% Feec-UNICAMP

% ---------------------------------------------------------

clear all;clc;

run('systemdata.m')

% Script for comparing MOESP and N4SID outputs

clc;

% Inputs of functions N, R, k

% Form Hankel matrices U,Y from system data (u,y) see blkhank.m

% R=input('Block_MOESP rows: ');

% k=input('Block_N4SID rows: ');

R=20;

k=20;

% call functions

[A1,B1,C1,D1,n1]=moespar(u,y,R);

[A2,B2,C2,D2,n2] = n4sidkatamodar(u,y,k);

[A3,B3,C3,D3,n3] = n4sid_self(u,y,k);

%Check Markov parameters (impulse responseof of MIMO system):

% Dr, Cr*Br, Cr*Ar*Br, Cr*Ar^2*Br ...

% D1, C1*B1, C1*A1*B1, C1*A1^2*B1 ...

% Graphics of system outputs and model outputs (estimate outputs)

run('modeloutputs.m')

% -------------------------------------------------------

% Generate system data (inputs and outputs)

% --------------------------------------------------------

% Author: Alexander Robles

% FEEC-UNICAMP

% ---------------------------------------------------------

clear all

clc

%   The state space system equations:

%                  x_k+1 = A x_k + B u_k

%                    y_k   = C x_k + D u_k

Ar=[0.2128 0.1360 0.1979 -0.0836;

0.1808 0.4420 -0.3279 0.2344;

-0.5182 0.1728 -0.5448 -0.3083;

0.2252 -0.0541 -0.4679 0.8290];

Br=[-0.0101 0.0317 -0.9347;

-0.0600 0.5621 0.1657;

-0.3310 -0.3712 -0.5846;

-0.2655 0.4255 0.2204];

Cr=[0.6557 -0.2502 -0.5188 -0.1229;

0.6532 -0.1583 -0.0550 -0.2497];

Dr=[-0.4326 0.1253 -1.1465

-1.6656 0.2877 1.1909];

%--------------

% N=input('Size of data N:');

N=500;

tt = 0:1:(N-1);

% -------------Input, u (white noise), is persistently exciting --------------

u1 = randn(1,N);

u2 = randn(1,N);

u3 = randn(1,N);

u = [u1; u2; u3];

%-------------Desing state-space model and outputs------

X0=zeros(4,1); % first state

%%%-----

x(:,1) = X0;

xc(:,1) = x(:,1);

yc(:,1) = Cr * xc(:,1) + Dr*u(:,1);

for i = 1:length(tt)

    xc(:,i+1) = Ar * xc(:,i) + Br* u(:,i);

    yc(:,i) = Cr * xc(:,i) + Dr*u(:,i);

    y = yc;

end

%-------------Plot inputs-------------------------

% figure (1)

% plot(tt,u1,'r',tt,u2,'c',tt,u3,'k');

% title('Inputs');

% legend('u1', 'u2', 'u3');

%-------------Plot outputs-------------------------

% figure (2)

% plot(tt,y(1,:),'g',tt,yc(1,:),'*b');

% title('Outputs');

% legend('y1', 'y1c');

%

% figure (3)

% plot(tt,y(2,:),'g',tt,yc(2,:),'*b');

% title('Outputs');

% legend('y2', 'y2c');

% MOESP Function 

% m = dim(u), p = dim(y), n = dim(x); R = Numero de filas de blocos

% U = km x N Matriz de Entradas

% Y = kp x N Matriz de Saidas

% Number of columns depends number of Hankel blocks R--- N=ndat-R+1

% If R ups >>j will decrease !DO NOT FORGET! 07/11/16

function [A,B,C,D,n] = moespar(u,y,R)

%Number of inputs and outputs:

[m,nu] = size(u);

if nu < m;u = u';[m,nu] = size(u);end

[p,ny] = size(y);

if ny < p;y = y';[p,ny] = size(y);end

Ncol=ny-R+1;% Calculate number of COLUMNS BLOCK HANKEL

%Make BLOCK HANKEL INPUTS U-OUTPUTS Y

U= blkhank(u,R,Ncol);

Y= blkhank(y,R,Ncol);

km = size(U,1); kp = size(Y,1);  % Rows of U and Y

L = triu(qr([U;Y]'))'; % LQ decomposition %Eq. 3.45  qr: Orthogonal-triangular decomposition.

L11 = L(1:km,1:km);

L21 = L(km+1:km+kp,1:km);

L22 = L(km+1:km+kp,km+1:km+kp);

[UU,SS,VV] = svd(L22);  % Eq. 3.48  Singular Value Descomposition

s1 = diag(SS);

n=find(cumsum(s1)>0.8*sum(s1),1);

%n=4;

U1 = UU(:,1:n); % n is known, as you can see last equation

Ok = U1*sqrtm(SS(1:n,1:n)); % SQRTM     Matrix square root. %Eq. 3.49

% Matrices A and C

C = Ok(1:p,1:n); % Eq. (6.41)

A = pinv(Ok(1:p*(R-1),1:n))*Ok(p+1:p*R,1:n);  % Eq.3.51

% Matrices B and D

U2 = UU(:,n+1:size(UU',1));

Z = U2'*L21/L11;  % Eq. 3.53

XX = []; RR = [];

for j = 1:R

XX = [XX; Z(:,m*(j-1)+1:m*j)];

Okj = Ok(1:p*(R-j),:);

Rj = [zeros(p*(j-1),p) zeros(p*(j-1),n);

eye(p) zeros(p,n); zeros(p*(R-j),p) Okj];

RR = [RR; U2'*Rj];

end

DB = pinv(RR)*XX;  % Eq. 3.57

D = DB(1:p,:);

B = DB(p+1:size(DB,1),:);

% Algoritmo do metodo N4SID modificado, com dados de entrada u,

% saida y, e k a ordem dos blocos de Hankel

function [A,B,C,D,n] = n4sidkatamodar(u,y,k)

[l,ny] = size(y);if (ny < l);y = y';[l,ny] = size(y);end

[m,nu] = size(u);if (nu < m);u = u';[m,nu] = size(u);end

% Determinar o numero de columns para as matrizes de Hankel

N = ny-2*k+1;

%Constroir as matrizes de dados, bloco de Hankel

U = blkhank(u,2*k,N);

Y = blkhank(y,2*k,N);

Up=U(1:k*m,:);

Uf=U(k*m+1:2*k*m,:);

Yp=Y(1:k*l,:);

Yf=Y(k*l+1:2*k*l,:);

km = size(Up,1);

kl = size(Yp,1);

Wp = [Up;Yp];

% *********** ALGORITMO ***********

%Passo 1

%decomposicao LQ

[Q,L] = qr([Uf;Up;Yp;Yf]',0);

Q=Q'; L=L';

L11 = L(1:km,1:km);

L21 = L(km+1:2*km,1:km);

L22 = L(km+1:2*km,km+1:2*km);

L31 = L(2*km+1:2*km+kl,1:km);

L32 = L(2*km+1:2*km+kl,km+1:2*km);

L33 = L(2*km+1:2*km+kl,2*km+1:2*km+kl);

L41 = L(2*km+kl+1:2*km+2*kl,1:km);

L42 = L(2*km+kl+1:2*km+2*kl,km+1:2*km);

L43 = L(2*km+kl+1:2*km+2*kl,2*km+1:2*km+kl);

L44 = L(2*km+kl+1:2*km+2*kl,2*km+kl+1:2*km+2*kl);

R11 = L11;

R21 = [L21;L31];

R22 = [L22 zeros(km,kl); L32 L33];

R31 = L41;

R32 = [L42 L43];

xi = R32*pinv(R22)*Wp;

%Passo 2

[UU,SS,VV] = svd(xi);

ss = diag(SS);

n=find(cumsum(ss)>0.85*sum(ss),1);

%n=4;

% hold off

% figure(1)

% title('Valores singulares');

% xlabel('Ordem');

% plot(ss)

% pause;

% figure(2)

% title('Valores singulares');

% xlabel('Ordem');

% bar(ss)

% n = input(' Ordem do sistema ? ');

% while isempty(n)

% n = input(' Ordem do sistema ? ');

% end

U1 = UU(:,1:n);

S1 = SS(1:n,1:n);

V1 = VV(:,1:n);

%Matrizes A e C

Ok = U1*sqrtm(S1);

C = Ok(1:l,1:n);

A = pinv(Ok(1:l*(k-1),1:n))*Ok(l+1:l*k,1:n);

%Passo 3

%Matrizes B e D

TOEP = (R31 - R32*pinv(R22)*R21)*pinv(R11);

G = TOEP(:,1:m);

G0 = G(1:l,:);

G1 = G(l+1:2*l,:);

G2 = G(2*l+1:3*l,:);

G3 = G(3*l+1:4*l,:);

G4 = G(4*l+1:5*l,:);

D = G0;

Hk = [G1 G2;G2 G3;G3 G4];

Ok1 = Ok(1:3*l,:);

Ck = pinv(Ok1)*Hk;

B = Ck(:,1:m);

%% 自己尝试估计状态序列向量

X_f_estimate = sqrtm(S1) * V1';

X_f_fzy = X_f_estimate(:,2:end);

X_p_fzy = X_f_estimate(:,1:end-1);

u_fzy = Uf(1:m,1:end-1);

y_fzy = Yf(1:l,1:end-1);

input_fzy = [X_p_fzy;u_fzy];

output_fzy = [X_f_fzy;y_fzy];

predict_sys = output_fzy*input_fzy'/(input_fzy*input_fzy');

predict_A = predict_sys(1:n,1:n);

predict_B = predict_sys(1:n,n+1:end);

predict_C = predict_sys(n+1:end,1:n);

predict_D = predict_sys(n+1:end,n+1:end);

% disp(norm(predict_A-A))

% disp(norm(predict_B-B))

% disp(norm(predict_C-C))

% disp(norm(predict_D-D))

% 

% H = blkhank(y,i,j)

% 

% Description:

%          Make a block Hankel matrix with the data y 

%          containing i block-rows and j columns

%     

function H = blkhank(y,i,j)  % i=k=R is an abritary positive 

% Make a (block)-row vector out of y

[l,nd] = size(y);

if nd < l;y = y';[l,nd] = size(y);end

% Check dimensions

if i < 0;error('blkHank: i should be positive');end

if j < 0;error('blkHank: j should be positive');end

if j > nd-i+1;error('blkHank: j too big');end % j is less or iqual nd-i+1

% Make a block-Hankel matrix

H=zeros(l*i,j);

for k=1:i

	H((k-1)*l+1:k*l,:)=y(:,k:k+j-1);

end

%Benchmarck :

%   Consider a multivariable fourth order system a,b,c,d

%   with 3 inputs and 2 outputs:

clc;

    u1 = u;

%   The state space system equations:

%                  x_{k+1) = A x_k + B u_k         

%                    y_k   = C x_k + D u_k 

    %   Model output:

    ym = dlsim(A1,B1,C1,D1,u1)'; % MOESP output

    yn = dlsim(A2,B2,C2,D2,u1)'; % N4SID output

    y_self = dlsim(A3,B3,C3,D3,u1)'; % N4SID output

%-------------Plot system outputs and model outputs-------------------------

figure (1)

hold off;subplot;clf;

subplot(211);plot(tt,y(1,:),'r',tt,ym(1,:),'*b');

title('First ouput');

legend('System output', 'MOESP output');

subplot(212);plot(tt,y(2,:),'r',tt,ym(2,:),'*b');

legend('System output', 'MOESP output');

title('Second output');

figure (2)

hold off;subplot;clf;

subplot(211);plot(tt,y(1,:),'r',tt,yn(1,:),'*b');

title('First ouput');

legend('System output', 'M4SID output');

subplot(212);plot(tt,y(2,:),'r',tt,yn(2,:),'*b');

legend('System output', 'N4SID output');

title('Second output');

figure (3)

hold off;subplot;clf;

subplot(211);plot(tt,y(1,:),'r',tt,y_self(1,:),'*b');

title('First ouput');

legend('System output', 'M4SID output');

subplot(212);plot(tt,y(2,:),'r',tt,y_self(2,:),'*b');

legend('System output', 'N4SID output');

title('Second output');

%----------------Mean square error--------------------------------------------

 errMOESP = (sum((y(1,:)-ym(1,:)).^2)+sum((y(2,:)-ym(2,:)).^2))/N

 errN4SID = (sum((y(1,:)-yn(1,:)).^2)+sum((y(2,:)-yn(2,:)).^2))/N

 errN4SID_self = (sum((y(1,:)-y_self(1,:)).^2)+sum((y(2,:)-y_self(2,:)).^2))/N