function Output_LS=LS(zi,ui)% 定义最小二乘算法函数
global P Z U H K CO %定义全局变量
if isempty(Z) Z=zeros(1,4);% 判断系统输出端的观测值
else
for k=1:3 %如输出观测值不为O,就向前移动
Z(k)=Z(k+1);
end
Z(4)=zi;% 把新的观测值赋予Z(4)
end
if isempty(U) U=zeros(1,3);% 判断系统输入端的观测值
else
U(1)=U(2);U(2)=U(3);U(3)=ui; %如输入观测值不为O,就向前移动
end
H=[-Z(3) -Z(2) -Z(1) U(2) U(1)]';% 设定观测矩阵H
if isempty(C0) C0=[0.00000001 0 0 0 0];% 设定seta(0)的初始参数
end
e=eye(5);% 矩阵e为5维的单位阵
if isempty(P) P=10^9.*e; %设p(0)初始值
end
K=1/(1+H'*P*H)*P*H;% 最小二乘递推算法
P=(e-K*H')*P;
C1=C0'+K*(zi-H'*CO');
CO=C1';
Output_LS=CO';