研发埠

标题: UKF用于感应电机转速估计 [打印本页]

作者: 袁郎郎 时间: 2013-5-28 16:54
标题: UKF用于感应电机转速估计
本人做基于UKF的感应电机转速辨识的问题目前在UKF阶段遇到很大的问题，经过UKF算法后转速输出是0!!!本人菜鸟一枚，求助众位大神。UKF的M文件：function [sys,x0,str,ts]=ukfwgx(t,x,u,flag)switch flag, case 0 [sys,x0,str,ts]=mdlInitializeSizes; case 2 sys=mdlUpdates(t,x,u); case 3 sys=mdlOutputs(t,x,u); case {1,4,9} sys=[]; otherwise error([&#39;Unhandled flag=&#39;,num2str(flag)]);endfunction[sys,x0,str,ts]=mdlInitializeSizessizes=simsizes;sizes.NumContStates = 0;%连续状态的数目sizes.NumDiscStates = 30;%离散状态数目sizes.NumOutputs = 5; %输出端口数目sizes.NumInputs = 4; %输入端口的数目sizes.DirFeedthrough = 0;%是否直通的标志sizes.NumSampleTimes=1; %采样周期数目sys=simsizes(sizes);x0=[0;0;0;0;0; 1;0;0;0;0; 0;1;0;0;0; 0;0;5;0;0; 0;0;0;1;0; 0;0;0;0;1 ];str=[];ts=[0.0001 0];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=[x(1);x(2);x(3);x(4);x(5)]

=[x(6) x(7) x(8) x(9) x(10);x(11) x(12) x(13) x(14) x(15);x(16) x(17) x(18) x(19) x(20);x(21) x(22) x(23) x(24) x(25);x(26) x(27) x(28) x(29) x(30)];R=[350 0; 0 350]; 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=[1-T/Ts 0 T*Lm/(delta*Ls*Lr*Tr) x_estimate(5)*T*Lm/(delta*Ls*Lr) 0 ; 0 1-T/Ts -x_estimate(5)*T*Lm/(delta*Ls*Lr) T*Lm/(delta*Ls*Lr*Tr) 0 ; T*Lm/Tr 0 1-T/Tr -T*x_estimate(5) 0; 0 T*Lm/Tr T*x_estimate(5) 1-T/Tr 0; 0 0 0 0 1 ];B=[T/(delta*Ls) 0; 0 T/(delta*Ls); 0 0; 0 0; 0 0];H=[1 0 0 0 0; 0 1 0 0 0 ];U=[u(1);u(2)];%选取参数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,

&#39;;s(:,3)=x_estimate+cc*n(2,

&#39;;s(:,4)=x_estimate+cc*n(3,

&#39;;s(:,5)=x_estimate+cc*n(4,

&#39;;s(:,6)=x_estimate+cc*n(5,

&#39;;s(:,7)=x_estimate-cc*n(1,

&#39;;s(:,8)=x_estimate-cc*n(2,

&#39;;s(:,9)=x_estimate-cc*n(3,

&#39;;s(:,10)=x_estimate-cc*n(4,

&#39;;s(:,11)=x_estimate-cc*n(5,

&#39;;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)&#39;+wc(2)*(h(:,2)-xgj)*(h(:,2)-xgj)&#39;+wc(3)*(h(:,3)-xgj)*(h(:,3)-xgj)&#39;+wc(4)*(h(:,4)-xgj)*(h(:,4)-xgj)&#39;+wc(5)*(h(:,5)-xgj)*(h(:,5)-xgj)&#39;+wc(6)*(h(:,6)-xgj)*(h(:,6)-xgj)&#39;+wc(7)*(h(:,7)-xgj)*(h(:,7)-xgj)&#39;+wc(8)*(h(:,8)-xgj)*(h(:,8)-xgj)&#39;+wc(9)*(h(:,9)-xgj)*(h(:,9)-xgj)&#39;+wc(10)*(h(:,10)-xgj)*(h(:,10)-xgj)&#39;+wc(11)*(h(:,11)-xgj)*(h(:,11)-xgj)&#39;+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)&#39;+wc(2)*(y(:,2)-yk)*(y(:,2)-yk)&#39;+wc(3)*(y(:,3)-yk)*(y(:,3)-yk)&#39;+wc(4)*(y(:,4)-yk)*(y(:,4)-yk)&#39;+wc(5)*(y(:,5)-yk)*(y(:,5)-yk)&#39;+wc(6)*(y(:,6)-yk)*(y(:,6)-yk)&#39;+wc(7)*(y(:,7)-yk)*(y(:,7)-yk)&#39;+wc(8)*(y(:,8)-yk)*(y(:,8)-yk)&#39;+wc(9)*(y(:,9)-yk)*(y(:,9)-yk)&#39;+wc(10)*(y(:,10)-yk)*(y(:,10)-yk)&#39;+wc(11)*(y(:,11)-yk)*(y(:,11)-yk)&#39;+R;%计算互协方差pxy=wc(1)*(h(:,1)-xgj)*(y(:,1)-yk)&#39;+wc(2)*(h(:,2)-xgj)*(y(:,2)-yk)&#39;+wc(3)*(h(:,3)-xgj)*(y(:,3)-yk)&#39;+wc(4)*(h(:,4)-xgj)*(y(:,4)-yk)&#39;+wc(5)*(h(:,5)-xgj)*(y(:,5)-yk)&#39;+wc(6)*(h(:,6)-xgj)*(y(:,6)-yk)&#39;+wc(7)*(h(:,7)-xgj)*(y(:,7)-yk)&#39;+wc(8)*(h(:,8)-xgj)*(y(:,8)-yk)&#39;+wc(9)*(h(:,9)-xgj)*(y(:,9)-yk)&#39;+wc(10)*(h(:,10)-xgj)*(y(:,10)-yk)&#39;+wc(11)*(h(:,11)-xgj)*(y(:,11)-yk)&#39;;%卡尔曼增益矩阵kk=pxy*(inv(pyy));%校正状态估计x_estimate=xgj+kk*([u(3);u(4)]-yk);%更新误差协方差阵P=pk-kk*pyy*kk&#39;;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=[x(1);x(2);x(3);x(4);x(5)];

欢迎光临研发埠 (http://bbs.yanfabu.com/)