f_lms_fit.m

多目标跟踪 (三维)-算法matlab编制

function [a_rt,p_rt]=f_lms_fit(t,input_mes,end_time,r)
%批处理最小二乘拟合
Hk=[];
Zk=[];
Rk=
r_size=size(r);
rsize=r_size(1);
line_total=rsize;
Rk=r;
for i=1:end_time 
   column_total=(i-1)*rsize;
   ZEMA1=zeros(line_total,column_total);
   ZEMA2=zeros(column_total,line_total);
   temp1=(i-1)*t;
   temp2=[1 temp1 temp1^2/2 0 0 0;0 0 0 1 temp1 temp1^2/2];
   Hk=[Hk;temp2];
   Zk=[Zk;input_mes(:,i)];
   if i>1
       Rk=[Rk ZEMA2;ZEMA1 r];
   end
end
a_rt=inv(Hk'*inv(Rk)*Hk)*Hk'*inv(Rk)*Zk;
p_rt=inv(Hk'*inv(Rk)*Hk)