扩展卡尔曼滤波器的s函数编写?

扩展卡尔曼滤波器的s函数编写?
function [sys,x0,str,ts] = sekfs(t,x,u,flag)
switch flag,
case 0,
[sys,x0,str,ts]=mdlInitializeSizes;
case 2,
sys=mdlUpdate(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
sizes = simsizes;
sizes.NumContStates = 0;
sizes.NumDiscStates = 3;
sizes.NumOutputs = 1;
sizes.NumInputs = 1;
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
x0 = [4.5000 15.0000 15.0000];
str = [];
ts = 1e-6;
function sys=mdlUpdate(t,x,u)
global Qon;
Qon=1;
R=1e-2;
Q=Qon*diag([1e-2 1e-2 1e-2]);
G_basal = 4.5; % mmol/L
X_basal = 15; % mU/L
I_basal = 15; % mU/L
P1 = 0.028735; % min-1
P2 = 0.028344; % min-1
P3 = 5.035e-5; % mU/L
V1 = 12; % L
n = 5/54; % min
D = 5;
To=1e-6;
% for i=1:3
% for j=1:3
% P(i,j)=x(3*i+j);
% end
% end
P=[0.01 0 0; 0 0.01 0;0 0 0.01];
xp=zeros(3,size(x,2));
xp(1,1) = x(1)+(-P1 * (x(1) - G_basal) - (x(2)- X_basal) * x(1) + D)*To;
xp(2,1) = x(2)+(-P2 * (x(2) - X_basal) + P3 * (x(3) - I_basal))*To;
xp(3,1) = x(3)+(-n * x(3) + u(1) / V1)*To;
F=[ -P1-x(2),-x(1),0 ;0,-P2,P3;0,0,-n];
P=F*P*F'+Q;
H=[1,0,0];
K=P*H'*inv(H*P*H'+R);
D=u(1);
H=xp(1,1);
xp=xp+K*(D-H);
P=P-K*H*P;
%sys=[xp(1,1);xp(2,1);xp(3,1)];
for i=1:3
sys(i)=xp(i,1);
end
% for i=1:3
% for j=1:3
% sys(i,j)=P(3*i+j);
% end
% end
function sys=mdlOutputs(t,x,u)
sys =x(1);
Error in 'untitled/S-Function' while executing M-File S-function 'sekfs',flag = 2 (update),at time 0.MATLAB error message:Inner matrix dimensions must agree.
哪里错了?

H=[1,0,0];
K=P*H'*inv(H*P*H'+R);
D=u(1);
H=xp(1,1);
xp=xp+K*(D-H);
P=P-K*H*P;
H是一个3*1的矩阵,但在下面H=xp(1,1),这时又变成一个数了,后面xp=xp+K*(D-H);没有问题.但是P=P-K*H*P;这里就出现了矩阵维数不对.你换过来就行了.