matlab下的命令學習--最小二乘法main（自用）

matlab下的命令學習（自用）

---

Matlab的單步調試操作:1、設置斷點的紅點；2、運行；3、F10；

---

傳遞函數中的：iodelay

---

最小二乘法

---

%%%zoh 零階保持，理想化；tustin 採用雙線性逼近，間接設計法，區別參考此鏈接tustin-zoh

%%main
Gs = tf([0.5],[1 1 1]);  % Actual system%%傳遞函數Gs=0.5/s^2+s+1
Ts = 0.2;                %Sampling Time
Gz = c2d(Gs,Ts);     %傳遞函數離散化dsys=c2d(sys,ts,'method');傳函離散
	                  %zoh 零階保持， 假設控制輸入在採樣週期內爲常值，爲默認值。
					 %foh 一階保持器，假設控制輸入在採樣週期內爲線性。
					 %tustin 採用雙線性逼近。method用tustin替代
					 %matched 採用SISO系統的零極點匹配法
	                  %[num,den]=tfdata(dsys,'v'); 離散後提取分子分母
	        
%%
u = @(t) 2*exp(-0.1*t)*sin(2*pi/2.5*t);     % Input %建立一個關於t的輸入函數@(t)_
t = 0:0.2:50;                               % Time of Operation%運行時間0到50s間隔0.2
% for i=1:length(t)                %length(t)爲t的數組長度
% U(i) = u(t(i)); % Input
% end
U = wgn(1,length(t),2);        %wgn用於產生高斯白噪聲
                               %y = wgn(m,n,p) 產生一個m行n列的高斯白噪聲的矩陣，p以dBW爲單位指定輸出噪聲的強度
%% Calculations of Orders
num = get(Gz,'num');          %取Gz分子  出來是CELL格式
den = get(Gz,'den');          %取Gz分母  出來是CELL格式
delay = get(Gz,'iodelay');    %延遲
Az = cell2mat(den);   %元胞數組中的多個矩陣合併成一個矩陣               
Az = Az./Az(1);       %./點除 如果a、b是矩陣，a./b就是a、b中對應的每個元素相除，得到一個新的矩陣；如果a、b是兩個數，那麼a./b就是普通的除法     
                             % Contains [1 a1 a2 ... ana]
if delay >0
Bz = [zeros(1,delay),cell2mat(num)]./Az(1);     %zeros(m,n)返回一個m x n的零矩陣。 % Contains [zeros(d) b0 b1 b2 ... bnb]
else
Bz = cell2mat(num)./Az(1);      % Contains [zeros(d) b0 b1 b2 ... bnb]  
end

% To get delay "d ==  num of first zeros in Bz"
ind = find(Bz == 0);%find()函數的基本功能是返回向量或者矩陣中不爲0的元素的*位置*索引。
test = isempty(ind);%%判斷數列ind是否爲空。ind爲空返回1；ind非空返回0。
if test == 1
        d = 0;
elseif ind(1) == 1
        d = 1;
elseif ind(1)~= 1 
        d = 0;
end

if length(ind)>1
for i=1:length(ind)-1
    if ind(i+1)-ind(i) == 1
       d = i+1;
    else
        break
    end
end
end

B = Bz(d+1:end);        % Contains [b0 b1 b2 ... bnb] 
A = Az(2:end);                            % Contains [a1 a2 ... ana]

nb=length(B)-1;   %nb=1
na=length(A);     %na=2
nu = na+nb+1;%    %nu=4


%% Actual Output of the System
Y = lsim(Gz,U,t);  % [y,t,x]=lsim[sys,u,t,x0]針對線性是不變模型，給定任意輸入U，得到輸出響應。%%%%lsim(SYS, u) 計算/繪製系統SYS對輸入向量u的響應

%% Estimation (simulated as online, where i represents time)

Theta = zeros(nu,1);                      % Initial Parameters
P = 10^6 * eye(nu,nu);                    % Initial Covariance Matrix
Phi = zeros(1,nu);                        % Initial phi
for i = 1 : length(U)            %i代表時間  從1：251
[a(:,i),b(:,i),P,Theta,Phi,K(:,i)] = RecursiveLeastSquares(U(1:i),Y(1:i),d,nb,na,P,Theta,Phi,i);%RSL函數  在函數文件裏面定義
%%%[a,b,P,Theta,phi,K] = RecursiveLeastSquares(U,Y,d,nb,na,P,Theta,phi,n)%U輸入Y輸出d爲延時nb零點個數na極點個數K時間參數P,Theta,phi最後計算的a(:,i),b(:,i)K(:,i)第i列，系統估計的向量參數
end

%% Plots

% Convergence of parameters
Sa = size(a);
Sb = size(b);
colors = ['b','r','g','k'];

figure
hold on
for m = 1:Sa(1)
    plot(t,a(m,:),'color',colors(m),'LineWidth',1.5)
    plot(t,A(m)*ones(length(t),1),'--','color','k')
    grid on
    title('Convergance of paramters a_i')
    xlabel('time (sec.)')
    ylabel('Paramter')
end
legend('a_1','','a_2','','a_3','a_4','a_5')
figure
hold on
for m = 1:Sb(1)
    plot(t,b(m,:),'color',colors(m),'LineWidth',1.5)
    plot(t,B(m)*ones(length(t),1),'--','color','k')%ones生成251行1列全爲1的矩陣
    grid on
    title('Convergance of paramters b_i')
    xlabel('time (sec.)')
    ylabel('Paramter')
end
legend('b_0','','b_1','','b_2','b_3','b_4','b_5')

% Check the estimated T.F.
y = lsim(tf(b(:,end)',[1 a(:,end)'],Ts,'iodelay',d,'variable','z^-1'),U,t);
%tf(b(:,end)',[1 a(:,end)'],Ts,'iodelay',d,'variable','z^-1')離散傳遞函數
% Compare the actual and estimated systems outputs
figure
grid on 
hold on
plot(t,Y(1:end),'--','LineWidth',1.5)
plot(t,y(1:end),'o','LineWidth',1.5,'color','r')
xlabel('time (sec.)')
ylabel('Amplitude')
title('2^n^d Order Estimated System')
legend('System Output','2^n^d Order Estimated Output') 

function [a,b,P,Theta,phi,K] = RecursiveLeastSquares(U,Y,d,nb,na,P,Theta,phi,n)
% This function Identify the system parameters for a known system input and
% output.
% The system Transfer Function is as in the following form:
%
%         z^-d * (bo + b1*z^-1 + b2*z^-2 + ... + b_nb*z^-nb)
% G(z) =  ---------------------------------------------------
%                1 + a1*z^-1 + ... + a_na*z^-na
%
% OUTPUTS:
% a and b are vectors of the system estimated parameters.
% INPUTS:
% u : is the system input raw vector, y is the system output raw vector.
% d : is the delay.
% nb: is the number of zeros of the equired system model.
% na: is the number of poles of the equired system model.
% k : is the instant of time at which parameters are to be calculated.
% P,Theta,phi: are the last calculated ones.

nu = na+nb+1;                           % Number of unknowns


for j = 1:nu
        if j <= na % terms of y
            if (n-j)<=0
                phi(n,j) = 0;
            else
                phi(n,j) = -Y(n-j);
            end
        else       % terms of u
            if (n-d-(j-(na+1)))<=0
                phi(n,j) = 0;
            else
                phi(n,j) = U(n-d-(j-(na+1)));
            end
        end
end

                    % Estimation   
           
           K = P*phi(n,:)'*inv(1+phi(n,:)*P*phi(n,:)');
           Theta = Theta+K*(Y(n)-phi(n,:)*Theta);
           P = P-P*phi(n,:)'*inv(1+phi(n,:)*P*phi(n,:)')*phi(n,:)*P;
                    % Estimated System Parameters
           a = Theta(1:na);
           b = Theta(na+1:end);
end