卡尔曼滤波

maggie19811117

我在用卡尔曼滤波做异步电机的转子电阻辨识,用MATLAB编的M文件,但是有错,不知道为什么,想请教哈各位,谢谢了
function [sys, x0] = train(t,x,u,flag,T)
%input,u:Usa,Usb,Isa,Isb,speed
%state,x:Isa,Isb,Ira,Irb,Rr
%sample time:T
global Q R x_1 P_1 delta P h Y out;
%Motor parameters
Lr=0.07331;Ls=0.07131;Lm=0.06931;Rs=0.435;delta=1-Lm*Lm/(Ls*Lr);
P=[10 0 0 0 0;0 10 0 0 0;0 0 10 0 0;0 0 0 10 0;0 0 0 0 0.11];
Q=[1.5e-7 0 0 0 0;0 1.5e-7 0 0 0;0 0 1.5e-8 0 0;0 0 0 1.5e-8 0;0 0 0 0 1.5e-9];
R=[0.0002 0;0 0.0002];
%initial parameters and states
if flag==0
    x0=zeros(5,1);  K=zeros(5,2);
    %Updata discrete states
else  abs(flag)==2
        %Set measurement variable
        U=[u(1);u(2);u(5)];
        Y=[u(3);u(4)];
        %Prediction of state
        dif_F=[1-Rs/(delta*Ls)*T,Lm*Lm/(delta*Ls*Lr)*T*u(5),Lm*T/(delta*Ls*Lr)*x(5),Lm*T/(delta*Ls)*u(5),Lm/(delta*Ls*Lr)*x(3);
            -Lm*Lm/(delta*Ls*Lr)*T*u(5),1-Rs/(delta*Ls)*T,-Lm/(delta*Ls)*T*u(5),Lm*T/(delta*Ls*Lr)*x(5),Lm*T/(delta*Ls*Lr)*x(4);
            Lm*Rs/(delta*Ls*Lr)*T,-Lm*T/(delta*Lr)*u(5),1-T/(delta*Lr)*x(5),-T*u(5)/delta,-T*x(3)/(delta*Lr);
            Lm/(delta*Lr)*T*u(5),Lm*Rs/(delta*Ls*Lr)*T,T*u(5)/delta,1-T*x(5)/(delta*Lr),-T*x(4)/(delta*Lr);
            0,0,0,0,1];
        x_1=[diff_F(1,1)*x(1)+diff_F(1,2)*x(2)+diff_F(1,3)*x(3)+diff_F(1,4)*x(4);
            diff_F(2,1)*x(1)+diff_F(2,2)*x(2)+diff_F(2,3)*x(3)+diff_F(2,4)*x(4);
            diff_F(3,1)*x(1)+diff_F(3,2)*x(2)+diff_F(3,3)*x(3)+diff_F(3,4)*x(4);
            diff_(4,1)*x(1)+diff_F(4,2)*x(2)+diff_F(4,3)*x(3)+diff_F(4,4)*x(4);
            diff_F(5,5)*x(5)]+T*[u(1)/(delta*Ls);u(2)/(delta*Ls);-u(1)*Lm/(delta*Ls*Lr);-u(2)*Lm/(delta*Ls*Lr);0];
        %Estimation of error covariance matrix
        P_1=diff_F*P*diff_F+Q;
        %Calculation of h and diff_h
        h=[x(1);x(2)];
        diff_h=[1 0 0 0 0;0 1 0 0 0];
        %Computation of Kalman Filter
        K=P_1*diff_h'*inv(dif_h*P_1*diff_h'+R);
        %State estimetion
        out=x_1+K*(Y-h);
        sys=out;
        %Updata of the error covariance matrix
        P=P_1-K*diff_h*P_1;
    end
    if flag==3
            sys=out;
        else flag==4
            sys=(round(t/T)+1)*T;
    end