扩展卡尔曼滤波的编程求助,急><

jason8011

這是用s-函數寫的
為什麼總是有問題
可否請高手修改一下
我怎樣都改不過來
這是用卡尔曼滤波按制感应电动机

function [sys,x0] = dix(t,x,u,flag,T)
%input, u:Vds,Vqs,Ids,Iqs
%states,x:Ids,Iqs,Fids,Fiqs,speed
%output: Ids,Iqs,Fids,speed
%sample time:T
global Tr Ts Lr Ls Lh Kl Kr Rs Rr Q R GQG G x_l P_l K P h Y out;

%Moto parameters
Lr=0.0417;
Ls=0.0424;
Lh=0.041;
Rs=0.294;
Rr=0.156;
H_pole=6/2;
Tr=Lr/Rr;
Kl=(1-Lh*Lh/Ls/Lr)*Ls;
Kr=Rs+Lh*Lh*Rr/Lr/Lr;

%initial parameters and states
if flag==0
    x0=zeros(5,1);
    K=zeros(5,2);
   
    P=[ 1 0 0 0 0;
        0 1 0 0 0;
        0 0 1 0 0;
        0 0 0 1 0;
        0 0 0 0 1];
   
    G=[ 1e-1 0 0 0 0 0;
        0 1e-1 0 0 0 0;
        0 0 1e-1 0 0 0;
        0 0 0 1e-1 0 0;
        0 0 0 0 1e-1 0];
        
   
    Q=[ 1e-5 0 0 0 0;
        0 1e-5 0 0 0;
        0 0 1e-5 0 0;
        0 0 0 1e-5 0;
        0 0 0 0 1e-5];
   
    R=[0.001 0
        0 0.001];
    GQG=G*Q*G';
    sys=[0,5,5,4,0,0];
   
    %Update discrete states
elseif abs(flag)==2
   
    %Set measurement variable
    U=[u(1); u(2)];
    Y=[u(3); u(4)];
   
    %Prediction of state
    dif_F=[1-Kr/Kl*T,0,Lh*Rr/Lr/Lr/Kl*T,Lh/Lr*x(5)*H_pole/Kl*T,Lh/Lr*x(4)/Kl*T;
        0,1-Kr/Kl*T,-Lh/Lr*x(5)*H_pole/Kl*T,Lh*Rr/Lr/Lr/Kl*T,-Lh_K/Lr_K*x(3)/Kl_K*T;
        Lh/Tr*T,0,1-T/Tr,-x(5)*H_pole*T,-x(4)*T;
        0,Lh/Tr*T,x(5)*H_pole*T,1-T/Tr,x(3)*T;
        0,0,0,0,1];
   
    x_l=[dif_F(1,1)*x(1)+dif_F(1,3)*x(3)+dif_F(1,4)*x(4);
        dif_F(2,2)*x(2)+dif_F(2,3)*x(3)+dif_F(2,4)*x(4);
        dif_F(3,1)*x(1)+dif_F(3,3)*x(3)+dif_F(3,4)*x(4);
        dif_F(4,2)*x(2)+dif_F(4,3)*x(3)+dif_F(4,4)*x(4);
        dif_F(5,5)*x(5)]+T*[u(1)/Kl;u(2)/Kl;0;0;0];
        
    %Estimation of error covariance matrix
    GQG=GQG-dif_F*GQG*dif_F'*T;
    P_l=dif_F*P*dif_F'+GQG;
   
    %Calculation of h and dif_h
    h=[x(1);x(2)];
    dif_h=[1 0 0 0 0;0 1 0 0 0];
   
    %Calculation of Kalman Filter
    K=P_l*dif_h'*inv(dif_h*P_l*dif_h'+R);
    %Estimation of Kalman Filter
    out=x_l+K*(Y-h);
    sys=out;
    %Update of the error covariance matrix
    P=P_l-K*dif_h*P_l;

elseif flag==3
    sys=out;
elseif flag==4;
    sys=(round(t/T)+1)*T;
else
sys=[];
end