UKF用于感应电机转速估计
本人做基于UKF的感应电机转速辨识的问题目前在UKF阶段遇到很大的问题，经过UKF算法后转速输出是0!!!本人菜鸟一枚，求助众位大神。UKF的M文件：function =ukfwgx(t,x,u,flag)switch flag, case 0 =mdlInitializeSizes; case 2 sys=mdlUpdates(t,x,u); case 3 sys=mdlOutputs(t,x,u); case {1,4,9} sys=[]; otherwise error(['Unhandled flag=',num2str(flag)]);endfunction=mdlInitializeSizessizes=simsizes;sizes.NumContStates = 0;%连续状态的数目sizes.NumDiscStates = 30;%离散状态数目sizes.NumOutputs = 5; %输出端口数目sizes.NumInputs = 4; %输入端口的数目sizes.DirFeedthrough = 0;%是否直通的标志sizes.NumSampleTimes=1; %采样周期数目sys=simsizes(sizes);x0=;str=[];ts=;function sys=mdlUpdates(t,x,u)T=0.0001;Ls=0.423;Lr=0.479;Lm=0.421;Rs=5.27;Rr=5.07;delta=0.125;Tr=Lr/Rr; Ts=1/(Rs/(delta*Ls)+(1-delta)/(delta*Tr));%Ts=(delta*Ls)/(Rs+(((Lm/Lr)^2)*Rr));x_estimate=;P=;R=; Q=[ 0.001 0 0 0 0; 0 0.001 0 0 0; 0 0 0.0005 0 0; 0 0 0 0.0005 0; 0 0 0 0 10];A=;B=;H=;U=;%选取参数L a b k ntL=5;a=0.002;k=0;b=2;nt=a*a*(L+k)-L;cc=L+nt;cc=sqrt(abs(cc));%选取sigma点n=chol(P);s(:,1)=x_estimate;s(:,2)=x_estimate+cc*n(1,:)';s(:,3)=x_estimate+cc*n(2,:)';s(:,4)=x_estimate+cc*n(3,:)';s(:,5)=x_estimate+cc*n(4,:)';s(:,6)=x_estimate+cc*n(5,:)';s(:,7)=x_estimate-cc*n(1,:)';s(:,8)=x_estimate-cc*n(2,:)';s(:,9)=x_estimate-cc*n(3,:)';s(:,10)=x_estimate-cc*n(4,:)';s(:,11)=x_estimate-cc*n(5,:)';for i=1:11h(:,i)=A*s(:,i)+B*U;end%计算sigma点的均值和协方差所用到的加权值wm(1)=nt/(L+nt);wc(1)=nt/(L+nt)+(1-a*a+b);wm(2)=1/(2*(L+nt));wc(2)=1/(2*(L+nt));wm(3)=1/(2*(L+nt));wc(3)=1/(2*(L+nt));wm(4)=1/(2*(L+nt));wc(4)=1/(2*(L+nt));wm(5)=1/(2*(L+nt));wc(5)=1/(2*(L+nt));wm(6)=1/(2*(L+nt));wc(6)=1/(2*(L+nt));wm(7)=1/(2*(L+nt));wc(7)=1/(2*(L+nt));wm(8)=1/(2*(L+nt));wc(8)=1/(2*(L+nt));wm(9)=1/(2*(L+nt));wc(9)=1/(2*(L+nt));wm(10)=1/(2*(L+nt));wc(10)=1/(2*(L+nt));wm(11)=1/(2*(L+nt));wc(11)=1/(2*(L+nt));%采用加权计算非线性传播后的均值和方差xgj=wm(1)*h(:,1)+h(:,2)*wm(2)+h(:,3)*wm(3)+h(:,4)*wm(4)+h(:,5)*wm(5)+h(:,6)*wm(6)+h(:,7)*wm(7)+h(:,8)*wm(8)+h(:,9)*wm(9)+h(:,10)*wm(10)+h(:,11)*wm(11);pk=wc(1)*(h(:,1)-xgj)*(h(:,1)-xgj)'+wc(2)*(h(:,2)-xgj)*(h(:,2)-xgj)'+wc(3)*(h(:,3)-xgj)*(h(:,3)-xgj)'+wc(4)*(h(:,4)-xgj)*(h(:,4)-xgj)'+wc(5)*(h(:,5)-xgj)*(h(:,5)-xgj)'+wc(6)*(h(:,6)-xgj)*(h(:,6)-xgj)'+wc(7)*(h(:,7)-xgj)*(h(:,7)-xgj)'+wc(8)*(h(:,8)-xgj)*(h(:,8)-xgj)'+wc(9)*(h(:,9)-xgj)*(h(:,9)-xgj)'+wc(10)*(h(:,10)-xgj)*(h(:,10)-xgj)'+wc(11)*(h(:,11)-xgj)*(h(:,11)-xgj)'+Q;%计算观测量的均值和估计for i=1:11y(:,i)=H*h(:,i);endyk=wm(1)*y(:,1)+wm(2)*y(:,2)+wm(3)*y(:,3)+wm(4)*y(:,4)+wm(5)*y(:,5)+wm(6)*y(:,6)+wm(7)*y(:,7)+wm(8)*y(:,8)+wm(9)*y(:,9)+wm(10)*y(:,10)+wm(11)*y(:,11);%计算新息自协方差pyy=wc(1)*(y(:,1)-yk)*(y(:,1)-yk)'+wc(2)*(y(:,2)-yk)*(y(:,2)-yk)'+wc(3)*(y(:,3)-yk)*(y(:,3)-yk)'+wc(4)*(y(:,4)-yk)*(y(:,4)-yk)'+wc(5)*(y(:,5)-yk)*(y(:,5)-yk)'+wc(6)*(y(:,6)-yk)*(y(:,6)-yk)'+wc(7)*(y(:,7)-yk)*(y(:,7)-yk)'+wc(8)*(y(:,8)-yk)*(y(:,8)-yk)'+wc(9)*(y(:,9)-yk)*(y(:,9)-yk)'+wc(10)*(y(:,10)-yk)*(y(:,10)-yk)'+wc(11)*(y(:,11)-yk)*(y(:,11)-yk)'+R;%计算互协方差pxy=wc(1)*(h(:,1)-xgj)*(y(:,1)-yk)'+wc(2)*(h(:,2)-xgj)*(y(:,2)-yk)'+wc(3)*(h(:,3)-xgj)*(y(:,3)-yk)'+wc(4)*(h(:,4)-xgj)*(y(:,4)-yk)'+wc(5)*(h(:,5)-xgj)*(y(:,5)-yk)'+wc(6)*(h(:,6)-xgj)*(y(:,6)-yk)'+wc(7)*(h(:,7)-xgj)*(y(:,7)-yk)'+wc(8)*(h(:,8)-xgj)*(y(:,8)-yk)'+wc(9)*(h(:,9)-xgj)*(y(:,9)-yk)'+wc(10)*(h(:,10)-xgj)*(y(:,10)-yk)'+wc(11)*(h(:,11)-xgj)*(y(:,11)-yk)';%卡尔曼增益矩阵kk=pxy*(inv(pyy));%校正状态估计x_estimate=xgj+kk*(-yk);%更新误差协方差阵P=pk-kk*pyy*kk';x(1)=x_estimate(1);x(2)=x_estimate(2);x(3)=x_estimate(3);x(4)=x_estimate(4);x(5)=x_estimate(5);x(6)=P(1,1);x(7)=P(1,2);x(8)=P(1,3);x(9)=P(1,4);x(10)=P(1,5);x(11)=P(2,1);x(12)=P(2,2);x(13)=P(2,3);x(14)=P(2,4);x(15)=P(2,5);x(16)=P(3,1);x(17)=P(3,2);x(18)=P(3,3);x(19)=P(3,4);x(20)=P(3,5);x(21)=P(4,1);x(22)=P(4,2);x(23)=P(4,3);x(24)=P(4,4);x(25)=P(4,5);x(26)=P(5,1);x(27)=P(5,2);x(28)=P(5,3);x(29)=P(5,4);x(30)=P(5,5);sys=x;function sys=mdlOutputs(t,x,u)sys=;
页:
[1]