扩展卡尔曼滤波的编程求助,急><
這是用s-函數寫的為什麼總是有問題
可否請高手修改一下
我怎樣都改不過來
這是用卡尔曼滤波按制感应电动机
function = 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=;
%Update discrete states
elseif abs(flag)==2
%Set measurement variable
U=;
Y=;
%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*;
%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=;
dif_h=;
%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
页:
[1]