扩展卡尔曼滤波

tyler

我的M文件是这样的
function [sys,x0,str,ts] = kalman(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(['Unhandled flag =',num2str(flag)]);
end
function [sys,x0,str,ts]=mdlInitializeSizes(FEstmtx,Hmtx,Cmtx);
sizes = simsizes;
sizes.NumContStates  = 0;
sizes.NumDiscStates  = 5;
sizes.NumOutputs     = 2;
sizes.NumInputs      = 2;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
x0 =zeros(5,1);
str = [];
ts  = [1 0];
function sys=mdlUpdates(t,x,u,FEstmtx,Hmtx,Cmtx)
x(1)=Ialfa;x(2)=Ibeta;x(3)=Phira;x(4)=Phirb;x(5)=wr
STEP=1;%步长值
koff=12000;%开始采样
ts=0.001;%仿真输入采样周期
tsest=STEP*ts;%估计采样周期
N=1000;
%MAXSAMPLES=floor(max(size(wr)-koff)/STEP):%最大采样值
Rs=0.7384:Ls=0.127145:Lm=0.1241:Rr=0.7403:Lr=0.127145;%电机定值设定
%PMTXINIT=10:%初始状态估算误差协方差值
%QMTXINIT=1;%过程噪声协方差阵
%RMTXINIT=0.001:%测量噪声协方差阵
%计算矩阵
U=zeros(2,1);Y=U;A=zeros(5,5):C=[1 0 0 0 0;0 1 0 0 0]
%Xestmtx=[0;0;0 ;0]
PP=[10,0,0,0,0;
    0,10,0,0,0;
    0,0,10,0,0;
    0,0,0,10,0;
    0,0,0,0,10];         
Q=[1,0,0,0,0;
   0,1,0,0,0;
   0,0,1,0,0;
   0,0,0,1,0;
   0,0,0,0,200];
RR=[0.001 0;0 0.001];K=zeros(5,2)
%开始参数估计循环
for k=1:N;
  sample=k*STEP+koff%采样时间
U(1)=Va(sample);U(2)=Vb(sample);Y(1)=Ia(sample);Y(2)=Ib(sample)
A=[1-(Rs+(Lm/Lr)*(Lm/Lr)*Rr)*ts/(1-Lm*Lm/(Ls*Lr)),0,ts*Lm*Rr/((Ls-Lm*Lm/Lr)*Lr),x(5)*ts*Lm/(Ls*Lr-Lm*Lm),0;
   0, 1-(Rs+(Lm/Lr)*(Lm/Lr)*Rr)*ts/(1-Lm*Lm/(Ls*Lr)),-x(5)*ts*Lm/(Ls*Lr-Lm*Lm),ts*Lm*Rr/((Ls-Lm*Lm/Lr)*Lr),0;
    ts*Lm*Rr/Lr,0,1-ts*Rr/Lr,-ts*x(5),0;
   0,ts*Lm*Rr/Lr,ts*x(5),1-ts*Rr/Lr,0;
   0,0,0,0,1 ];
x=[A(1,1)*x(1)+A(1,3)*x(3)+A(1,4)*x(4)+ts/(Ls-Lm*Lm/Lr)*U(1);
   A(2,2)*x(2)+A(2,3)*x(3)+A(2,4)*x(4)+ts/(Ls-Lm*Lm/Lr)*U(1);
   A(3,1)*x(1)+A(3,3)*x(3)+A(3,4)*x(4);
   A(4,2)*x(2)+A(4,3)*x(3)+A(4,4)*x(4);
   A(5,5)*x(5) ];
G=[1-(Rs+(Lm/Lr)*(Lm/Lr)*Rr)*ts/(1-Lm*Lm/(Ls*Lr)),0,ts*Lm*Rr/((Ls-Lm*Lm/Lr)*Lr),x(5)*ts*Lm/(Ls*Lr-Lm*Lm),x(4)*ts*Lm/(Ls*Lr-Lm*Lm);
   0, 1-(Rs+(Lm/Lr)*(Lm/Lr)*Rr)*ts/(1-Lm*Lm/(Ls*Lr)),-x(5)*ts*Lm/(Ls*Lr-Lm*Lm),ts*Lm*Rr/((Ls-Lm*Lm/Lr)*Lr),-x(3)*ts*Lm/(Ls*Lr-Lm*Lm);
    ts*Lm*Rr/Lr,0,1-ts*Rr/Lr,-ts*x(5),ts*x(4);
   0,ts*Lm*Rr/Lr,ts*x(5),1-ts*Rr/Lr,ts*x(4);
   0,0,0,0,1 ];
H=[1,0,0,0,0;
   0,1,0,0,0];
C=[1,0,0,0,0;
   0,1,0,0,0];
%根据扩展卡尔曼滤波算法计算
P=G*PP*G'+RR;
K=P*H'*inv(H*P*H'+RR);
x=x+K*(Y-C*x);
PP=P+K*H*P;
%end
%保存变量
Ialfa(k+1)=x(1);
Ibeta(k+1)=x(2);
phira(k+1)=x(3);
phirb(k+1)=x(4);
wr(k+1)=x(5);
end
function sys=mdlOutputs(t,x,u,A,B,C)
sys=x(5);
S-FUNCTION是这样的
提示错误信息Error evaluating parameter 'FunctionName' in block 'resistor1/S-Function': Undefined variable 'myekf' or class 'myekf.m'

[ 本帖最后由 tyler 于 2007-9-9 10:35 编辑 ]

tyler

大家帮帮忙啊，谢谢了！

lwfyvwancumt

你加我QQ：531659007     注明：sensorless
我在一篇辽宁工程大学的硕士论文中看到过这段程序
说实话有几处看不懂
我自己写了一个S函数
现在可以用   有空聊聊

jason_boy

我也在做这方面的 多交流啊

qingyifanfei

你的问题解决了没有？我最近也在做关于这方面的毕设，在编EKF的程序上出了点问题。但是解决不了。想问问你啊。能不能留下QQ让我加下请教下啊