%遞推最小二乘參數(shù)辨識rlspara.m
clear all%清理工作間變量

N1=1372;%基本周期
num=1;%周期數(shù)
N=N1*num;%算法使用的數(shù)據(jù)點數(shù)

%模擬電流的變化
I1=(96.5*randn(1,N1)+100)/4; 
I=[I1];% I1 I1 I1 I1 I1 I1 I1];

for n=1:N
    %放電電流
    if I(n)>0 
        u1(n)=I(n);
        u2(n)=0;
    %充電電流
    else
        u1(n)=0;
        u2(n)=I(n);
    end
    %模擬SOC的變化，假設呈現(xiàn)線性變化
     u3(n)=0.643-0.409*10^-4*(n-1);
     u4(n)=1/u3(n);  %1/SOC
     u5(n)=log(u3(n)); %ln(SOC)
     u6(n)=log(1-u3(n)); %ln(1-SOC)
     %模擬輸出即電壓的變化
     y(n) = 902.66-0.2594*u1(n)-0.2288*u2(n)-1088.3*u3(n)+33.482*u4(n)+320.64*u5(n)-307.3*u6(n);
end
y=y+randn(1,N);
%遞推最小二乘辨識
c0=[0.001 0.001 0.001 0.001 0.001 0.001 0.001]'; %直接給出被辨識參數(shù)的初始值,即一個充分小的實向量
p0=10^6*eye(7,7); %直接給出初始狀態(tài)P0，即一個充分大的實數(shù)單位矩陣
% E=0.000000005; %取相對誤差E=0.000000005
c=[c0,zeros(7,N-1)]; %被辨識參數(shù)矩陣的初始值及大小
e=zeros(7,N); %相對誤差的初始值及大小
for k=1:N; %開始求K 
    h=[1,u1(k),u2(k),u3(k),u4(k),u5(k),u6(k)]'; x=h'*p0*h+1; x1=inv(x); %開始求K(k)
    Kk=p0*h*x1;%求出K的值
    d1=y(k)-h'*c0; c1=c0+Kk*d1; %求被辨識參數(shù)c
    e1=c1-c0; %求參數(shù)當前值與上一次的值的差值
    e2=e1./c0; %求參數(shù)的相對變化
    e(:,k)=e2; %把當前相對變化的列向量加入誤差矩陣的最后一列       
    c0=c1; %新獲得的參數(shù)作為下一次遞推的舊參數(shù)
    c(:,k)=c1; %把辨識參數(shù)c 列向量加入辨識參數(shù)矩陣的最后一列 
    p1=p0-x1*p0*h*h'*p0;%求出 p(k)的值
    %p1=p0-k1*k1'*[h1'*p0*h1+1]; 
    p0=p1; %給下次用
%     if e2<=E 
%       break; %如果參數(shù)收斂情況滿足要求，終止計算
%     end %小循環(huán)結束
end %大循環(huán)結束
% 分離參數(shù)
K0=c(1,:); Rd=c(2,:); Rc=c(3,:); K1=c(4,:); K2=c(5,:); K3=c(6,:); K4=c(7,:); 
eK0=e(1,:); eRd=e(2,:); eRc=e(3,:); eK1=e(4,:);eK2=e(5,:); eK3=e(6,:); eK4=e(7,:);  
figure(1); %第1個圖形
i=1:N; 
plot(i,K0,'r',i,Rd,':',i,Rc,'g',i,K1,':',i,K2,':',i,K3,':',i,K4,':')  %畫出K0,Rd,Rc,K1,K2,K3,K4的各次辨識結果
title('Parameter Identification with Recursive Least Squares Method') %圖形標題
figure(2); %第2個圖形
i=1:N; 
plot(i,eK0,'r',i,eRd,'g',i,eRc,'b',i,eK1,'r:',i,eK2,'r:',i,eK3,'r:',i,eK4,'r:') %畫出K0,Rd,Rc,K1,K2,K3,K4的各次辨識結果的收斂情況
title('Identification Precision') %圖形標題