首页 控制工程

# 一、N4SID 算法概述

  N4SID (Subspace Identification for State-Space Models) 方法是系统辨识领域中一种常用的子空间方法，用于从输入输出数据中辨识线性时不变系统的状态空间模型。该方法的主要目标是通过测量数据来确定系统的状态、动态矩阵和输出矩阵，从而获得系统的数学模型。这种方法广泛应用于自动控制、信号处理、通讯等领域，尤其适用于多输入多输出 (MIMO) 系统。N4SID 方法通过子空间技术，能够有效地处理高阶、多变量系统的辨识问题。它无需假设特定的系统结构，直接从数据中提取状态信息，避免了许多传统方法中的参数估计困难和计算复杂度问题。

# 二、N4SID 算法主要流程

# 2.1 数据预处理

  考虑如下的线性时不变离散状态空间方程模型：

{xk+1=Axk+Bukyk=Cxk+Duk\left\{\begin{matrix} \mathbf{x}_{k+1}=\mathbf{A}\mathbf{x}_k+\mathbf{B}\mathbf{u}_k \\ \mathbf{y}_{k}=\mathbf{C}\mathbf{x}_k+\mathbf{D}\mathbf{u}_k \end{matrix}\right.

  其中xk∈Rn\mathbf{x}_k\in \mathcal{R}^{n}，uk∈Rm\mathbf{u}_k\in \mathcal{R}^{m} 和yk∈Rl\mathbf{y}_k\in \mathcal{R}^{l} 分别是系统在kk 时刻的状态向量、输入向量和输出向量。

  对u\mathbf{u} 的所有维度添加高斯白噪声，并采集nTn_T 个时刻的uk\mathbf{u}_k 和yk\mathbf{y}_k。（注：论文《N4SID: Subspace Algorithms for the Identi cation of Combined Deterministic-Stochastic Systems》的 Section 6.1 中提到了当采样时间无限，或确定性输入u\mathbf{u} 是白色噪声，或系统是纯确定性系统时，N4SID 的估计是无偏估计）

# 2.2 构造 Hankel 矩阵

  根据 2.1 中采集的数据uk\mathbf{u}_k 和yk\mathbf{y}_k, k=1,...nTk=1,...n_T，设计 Hankel 矩阵的行数2∗k2*k，则整体 Hankel 矩阵的列数为N=nT−2∗k+1N=n_T-2*k+1。将整体的 Hankel 矩阵按照行数对半拆分，可以获得 Hankel 矩阵Up\mathbf{U}_p，Uf\mathbf{U}_f，Yp\mathbf{Y}_p 和Yf\mathbf{Y}_f，分别如下所示：

  过去输入 / 输出 Hankel 矩阵

Up=[u(0)u(1)⋯u(N−1)u(1)u(2)⋯u(N)⋮⋮⋱⋮u(k−1)u(k)⋯u(k+N−2)]\mathbf{U}_p=\begin{bmatrix} u(0) & u(1) & \cdots & u(N-1)\\ u(1) & u(2) & \cdots & u(N)\\ \vdots & \vdots & \ddots & \vdots \\ u(k-1) & u(k) & \cdots & u(k+N-2) \end{bmatrix}

Yp=[y(0)y(1)⋯y(N−1)y(1)y(2)⋯y(N)⋮⋮⋱⋮y(k−1)y(k)⋯y(k+N−2)]\mathbf{Y}_p=\begin{bmatrix} y(0) & y(1) & \cdots & y(N-1)\\ y(1) & y(2) & \cdots & y(N)\\ \vdots & \vdots & \ddots & \vdots \\ y(k-1) & y(k) & \cdots & y(k+N-2) \end{bmatrix}

  未来输入 / 输出 Hankel 矩阵

Uf=[u(k)u(k+1)⋯u(k+N−1)u(k+1)u(k+2)⋯u(k+N)⋮⋮⋱⋮u(2k−1)u(2k)⋯u(2k+N−2)]\mathbf{U}_f=\begin{bmatrix} u(k) & u(k+1) & \cdots & u(k+N-1)\\ u(k+1) & u(k+2) & \cdots & u(k+N)\\ \vdots & \vdots & \ddots & \vdots \\ u(2k-1) & u(2k) & \cdots & u(2k+N-2) \end{bmatrix}

Yf=[y(k)y(k+1)⋯y(k+N−1)y(k+1)y(k+2)⋯y(k+N)⋮⋮⋱⋮y(2k−1)y(2k)⋯y(2k+N−2)]\mathbf{Y}_f=\begin{bmatrix} y(k) & y(k+1) & \cdots & y(k+N-1)\\ y(k+1) & y(k+2) & \cdots & y(k+N)\\ \vdots & \vdots & \ddots & \vdots \\ y(2k-1) & y(2k) & \cdots & y(2k+N-2) \end{bmatrix}

# 2.3 正交三角分解（QR）

  定义Wp=[UpYp]\mathbf{W}_p=\begin{bmatrix} \mathbf{U}_p \\ \mathbf{Y}_p \end{bmatrix}，考虑如下的下三角正交 LQ 分解：

[UfWpYf]=[R1100R21R220R31R32R33][Q1TQ2TQ3T]\left[\begin{array}{c} \mathbf{U}_{f} \\ \mathbf{W}_{p} \\ \mathbf{Y}_{f} \end{array}\right]=\left[\begin{array}{ccc} \mathbf{R}_{11} & \mathbf{0} & \mathbf{0} \\ \mathbf{R}_{21} & \mathbf{R}_{22} & \mathbf{0} \\ \mathbf{R}_{31} & \mathbf{R}_{32} & \mathbf{R}_{33} \end{array}\right]\left[\begin{array}{c} \mathbf{Q}_1^{\mathrm{T}} \\ \mathbf{Q}_2^{\mathrm{T}} \\ \mathbf{Q}_3^{\mathrm{T}} \end{array}\right]

  式中，R11\mathbf{R}_{11}、R22\mathbf{R}_{22} 为下三角矩阵，R33=0\mathbf{R}_{33}=0，Q1\mathbf{Q}_1、Q2\mathbf{Q}_2 正交。从上式可以得出：

Yf=R32R22−1Wp+(R31−R32R22−1R21)R11−1Uf\mathbf{Y}_f=\mathbf{R}_{32}\mathbf{R}_{22}^{-1}\mathbf{W}_p+(\mathbf{R}_{31}-\mathbf{R}_{32}\mathbf{R}_{22}^{-1}\mathbf{R}_{21})\mathbf{R}_{11}^{-1}\mathbf{U}_f

# 2.4 奇异值分解（SVD）

  原始系统的 Toeplitz 矩阵为：

ψk=[D0⋯0CBD⋯0⋮⋮⋱⋮CAk−2BCAk−3B⋯D]\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]

  扩展可观性矩阵为：

Γk=[CCA⋮CAk−1]\mathbf{\Gamma} _k=\begin{bmatrix} \mathbf{C} \\ \mathbf{CA} \\ \vdots \\ \mathbf{CA^{k-1}} \end{bmatrix}

  过去状态序列为：

Xp=[x(0)x(1)⋯x(N−1)]\mathbf{X}_p=\begin{bmatrix} x(0) & x(1) & \cdots x(N-1) & \end{bmatrix}

  未来状态序列为：

Xf=[x(k)x(k+1)⋯x(k+N−1)]\mathbf{X}_f=\begin{bmatrix} x(k) & x(k+1) & \cdots x(k+N-1) & \end{bmatrix}

  根据 2.2 节和如上的定义，可以得到：

Yp=ΓkXp+ψkUp\mathbf{Y}_p=\mathbf{\Gamma} _k\mathbf{X}_p+\mathbf{\psi}_k\mathbf{U}_p

Yf=ΓkXf+ψkUf\mathbf{Y}_f=\mathbf{\Gamma} _k\mathbf{X}_f+\mathbf{\psi}_k\mathbf{U}_f

  对比上式和 2.3 节中最后一个式子，可以得到：

ξ=ΓkXf=R32R22−1Wp\mathbf{\xi}=\mathbf{\Gamma} _k\mathbf{X}_f=\mathbf{R}_{32}\mathbf{R}_{22}^{-1}\mathbf{W}_p

  对ξ\mathbf{\xi} 进行 SVD 分解，可以得到：

ξ=[U1U2][Σ1000][V1TV2T]=U1Σ1V1T\mathbf{\xi}=\left[\begin{array}{ll} \mathbf{U}_1 & \mathbf{U}_2 \end{array}\right]\left[\begin{array}{cc} \mathbf{\Sigma}_1 & \mathbf{0} \\ \mathbf{0} & \mathbf{0} \end{array}\right]\left[\begin{array}{l} \mathbf{V}_1^{\mathrm{T}} \\ \mathbf{V}_2^{\mathrm{T}} \end{array}\right]=\mathbf{U}_1 \mathbf{\Sigma}_1 \mathbf{V}_1^{\mathrm{T}}

# 2.5 （方法 1）估计状态序列X^f\hat{\mathbf{X}} _f

  从 2.4 节中的 SVD 分解结果得到如下关系：

Γk=U1Σ11/2T,∣T∣≠0\mathbf{\Gamma}_k=\mathbf{U}_1 \mathbf{\Sigma}_1^{1 / 2} \mathbf{T}, \quad|\mathbf{T}| \neq 0

Xf=T−1Σ11/2V1T\mathbf{X}_{\mathrm{f}}=\mathbf{T}^{-1} \mathbf{\Sigma}_1^{1 / 2} \mathbf{V}_1^{\mathrm{T}}

  一般情况下，可以取T=I\mathbf{T}=\mathbf{I}。根据上式可以估计出状态序列向量X^f\hat{\mathbf{X}} _f：

X^f=[x^(k)x^(k+1)⋯x^(k+N−1)]\hat{\mathbf{X}} _f=\begin{bmatrix} \hat{x}(k) & \hat{x}(k+1) & \cdots \hat{x}(k+N-1) & \end{bmatrix}

定义N−1N-1 维向量：

X^f(k)=[x^(k)x^(k+1)⋯x^(k+N−2)]\hat{\mathbf{X}} _f ^{(k)}=\begin{bmatrix} \hat{x}(k) & \hat{x}(k+1) & \cdots \hat{x}(k+N-2) & \end{bmatrix}

X^f(k+1)=[x^(k+1)x^(k+2)⋯x^(k+N−1)]\hat{\mathbf{X}} _f ^{(k+1)}=\begin{bmatrix} \hat{x}(k+1) & \hat{x}(k+2) & \cdots \hat{x}(k+N-1) & \end{bmatrix}

Uk∣k=[u(k)u(k+1)⋯u(k+N−2)]{\mathbf{U}} _{k|k}=\begin{bmatrix} {u}(k) & {u}(k+1) & \cdots {u}(k+N-2) & \end{bmatrix}

Yk∣k=[y(k)y(k+1)⋯y(k+N−2)]{\mathbf{Y}} _{k|k}=\begin{bmatrix} {y}(k) & {y}(k+1) & \cdots {y}(k+N-2) & \end{bmatrix}

# 2.6 （方法 1）最小二乘法计算系统参数

  根据 2.5 节估计的状态序列向量，可以构造如下的关系：

[X^k+1Yk∣k]=[A^B^C^D^][X^kUk∣k]\left[\begin{array}{c} \hat{\mathbf{X}}_{k+1} \\ {\mathbf{Y}}_{k \mid k} \end{array}\right]=\left[\begin{array}{cc} \mathbf{\hat{A}} & \mathbf{\hat{B}} \\ \mathbf{\hat{C}} & \mathbf{\hat{D}} \end{array}\right]\left[\begin{array}{c} \hat{\mathbf{X}}_k \\ {\mathbf{U}}_{k \mid k} \end{array}\right]

  这是一个系统的线性矩阵方程，可以使用最小二乘法估计出系统的矩阵参数

[A^B^C^D^]=([X^k+1Yk∣k][X^kUk∣k]T)([X^kUk∣k][X^kUk∣k]T)−1\left[\begin{array}{cc} \hat{\mathbf{A}} & \hat{\mathbf{B}} \\ \hat{\mathbf{C}} & \hat{\mathbf{D}} \end{array}\right]=\left(\left[\begin{array}{c} \hat{\mathbf{X}}_{k+1} \\ {\mathbf{Y}}_{k \mid k} \end{array}\right]\left[\begin{array}{c} \hat{\mathbf{X}}_k \\ {\mathbf{U}}_{k \mid k} \end{array}\right]^{\mathrm{T}}\right)\left(\left[\begin{array}{c} \hat{\mathbf{X}}_k \\ {\mathbf{U}}_{k \mid k} \end{array}\right]\left[\begin{array}{c} \hat{\mathbf{X}}_k \\ {\mathbf{U}}_{k \mid k} \end{array}\right]^{\mathrm{T}}\right)^{-1}

# 2.5 （方法 2）直接使用 SVD 的结果计算系统参数矩阵

  由于

Γk=U1Σ11/2T,∣T∣≠0\mathbf{\Gamma}_k=\mathbf{U}_1 \mathbf{\Sigma}_1^{1 / 2} \mathbf{T}, \quad|\mathbf{T}| \neq 0

Γk=[CCA⋮CAk−1]\mathbf{\Gamma} _k=\begin{bmatrix} \mathbf{C} \\ \mathbf{CA} \\ \vdots \\ \mathbf{CA^{k-1}} \end{bmatrix}

  因此可以从计算出的Γk\mathbf{\Gamma}_k 中提取矩阵C^\hat{\mathbf{C}}。矩阵A^\hat{\mathbf{A}} 可以通过下式进行计算，其中上标−1^{-1} 在矩阵不为方阵时可采用伪逆的计算方式：

A^=[CCA⋮CAk−2]−1[CACA2⋮CAk−1]\hat{\mathbf{A}}=\begin{bmatrix} \mathbf{C} \\ \mathbf{CA} \\ \vdots \\ \mathbf{CA^{k-2}} \end{bmatrix}^{-1}\begin{bmatrix} \mathbf{CA} \\ \mathbf{CA^2} \\ \vdots \\ \mathbf{CA^{k-1}} \end{bmatrix}

  同理，由于

ψk=(R31−R32R22−1R21)R11−1\mathbf{\psi}_k=(\mathbf{R}_{31}-\mathbf{R}_{32}\mathbf{R}_{22}^{-1}\mathbf{R}_{21})\mathbf{R}_{11}^{-1}

ψk=[D0⋯0CBD⋯0⋮⋮⋱⋮CAk−2BCAk−3B⋯D]\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]

  因此可以从计算出的ψk\mathbf{\psi}_k 中提取矩阵D^\hat{\mathbf{D}}，矩阵B^\hat{\mathbf{B}} 可以通过下式进行计算，其中上标−1^{-1} 在矩阵不为方阵时可采用伪逆的计算方式：

B^=[CCA⋮CAk−2]−1[CBCAB⋮CAk−2B]\hat{\mathbf{B}}=\begin{bmatrix} \mathbf{C} \\ \mathbf{CA} \\ \vdots \\ \mathbf{CA^{k-2}} \end{bmatrix}^{-1}\begin{bmatrix} \mathbf{CB} \\ \mathbf{CAB} \\ \vdots \\ \mathbf{CA^{k-2}B} \end{bmatrix}

# 三、参考 matlab 程序

setup_ti.m
% --------------------------------------------------------
% 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')
systemdata.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');
moespar.m
% 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),:);
n4sidkatamodar.m
% 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))
blkhank.m
% 
% 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
modeloutputs.m
%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

      

          
      

  


   


    

  
    
      
    
    更新于
    
  
  
      
        
      
      阅读次数
      
      次