start_callback.m

拥有matlab用户界面的卡尔曼滤波程序

function start_callback

try 
    load inidata;
catch
    msgbox('The filter has not been initialized!','Warning','warn','modal');
    return
end

delete(gca);
setaxis;
Pk_1=P0;
Xk_1=X0;
k=1;
order=length(A);

BII=findobj(gcf,'tag','start');
mp=get(BII,'userdata');
set(BII,'userdata',mp+.0001);%to end last 'start circle'
mp=get(BII,'userdata');

BIII=findobj(gcf,'tag','stop');
set(BIII,'userdata',1,'String','Stop?');

M=findobj(gcf,'tag','time');
t=get(M,'userdata');

CI=findobj(gcf,'tag','compo1');
if isempty(get(CI,'userdata'))
  set(CI,'userdata',1:order);
end
co=get(CI,'userdata');%co decides the output components

CII=findobj(gcf,'tag','p');
cii=get(CII,'userdata');
if cii==1
    si=1;
else
    si=2;%si decides whether to output error or state
end

L=findobj(gcf,'tag','legendh');
set(L,'userdata',0);

load fZk;% load mesurements


%%%%%%%%%%%%%%%%%%
% Filtering Loop %
%%%%%%%%%%%%%%%%%%
try
    while k<=t(2) 
    if get(BII,'userdata')~=mp%to end last 'start circle'
        break;
    end
       Phik_=A;           
       Zk=Zkk{k};%Zkk is from fZk
       Gamak_=B;
       Hk=H;
       Qk_1=Q;
       Rk=R;
              
       Pk_=Phik_*Pk_1*Phik_'+Gamak_*Qk_1*Gamak_';
       Kk=Pk_*Hk'*(Hk*Pk_*Hk'+Rk)^(-1);
       I=eye(length(Pk_));
       Pkk=(I-Kk*Hk)*Pk_*(I-Kk*Hk)'+Kk*Rk*Kk';
    
       Xk_=Phik_*Xk_1;
       Xk=Xk_+Kk*(Zk-Hk*Xk_);
       
       drawing(Pk_1,Pkk,Xk_1,Xk,k,co,si);    
             
       Pk_1=Pkk;   
       Xk_1=Xk;
       k=k+1;
   end
catch
    return
end