扩展卡尔曼滤波器的s函数编写
function [sys,x0,str,ts] = kfs(t,x,u,flag)
switch flag,
ca 0,
[sys,x0,str,ts]=mdlInitializeSizes;
ca 2,
sys=mdlUpdate(t,x,u);
ca 3,
sys=mdlOutputs(t,x,u);
ca {1,4,9},
sys=[];
otherwi
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 'kfs', 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;这里就出现了矩阵维数不对。你换过来就行了。
你的调用要是在for循环体中，那么你的输入参数比如x是否进行矩阵维数扩展，即表示成x(1,k)这种形式？（这条可能就是产生问题的最主要原因）
卷发棒
另外x和u的维数是否正确？我看这个函数程序中的x应该是3维的列向量
还有你函数中xp=zeros(3,size(x,2));这句所生成的是一个三维的列向量，那么下面的xp(1,1)..xp(3,1)这些调用是否合理等都需要认真考虑
function [sys,x0,str,ts] = str_sim(t,x,z,flag,T,n,R1,xinit,Pinit)
%EKFS Extended Kalman filter S-function for rekursive identification
% This M-file is designed to be ud in a Simulink S-function block.
% It implements an extended Kalman filter for joint state estimation
% and parameter tracking. Sofar, only output error SISO models are 女句
% supported
%
% Input: theta
% Output: (dummy)
%
switch flag,
%%%%%%%%%%%%%%%%%%
% Initialization %
%%%%%%%%%%%%%%%%%%
ca 0,
% M鰆ligen kan jag komma p?n錱on smart initialiring h鋜...
x0 = [xinit(:); Pinit(:)]; % init the state vector
ts = [T,0];
sys(1) = 0; % 0 continuous states
sys(2) = length(x0); % enough discrete states to hold x and P
sys(3) = 3*n; % All states as outputs
sys(4) = 2; % inputs: z = [y u]
sys(5) = 0; % 0 roots
sys(6) = 1; % Direct feedthrough
sys(7) = 1; % 1 sample time
感动的近义词 %%%%%%%%%%
% Update %
%%%%%%%%%%
ca 2,
xs = x(1:3*n,1);
P = zeros(3*n,3*n);
P(:) = x(3*n+1:end,1);
% Measurement update
y = z(1);
u = z(2);
R = 1;
H = [1 zeros(1,n-1+2*n)];
K = P*H'/(H*P*H'+R); % only SISO models supported
xs = xs + K*(y-H*xs);中华文化的内容
% Time update
Q = eye(2*n)*R1;
a = xs(n+1:2*n);
A = [-a(:) [eye(n-1);zeros(1,n-1)]];
F = [A -xs(1)*eye(n) u*eye(n);zeros(2*n,n) eye(2*n)];
G = [zeros(n,2*n); eye(2*n)];
P = F*(P-K*H*P)*F'+G*Q*G';
xs(1:n) = [-xs(n+1:2*n) [eye(n-1);zeros(1,n-1)]]*xs(1:n) + xs(2*n+1:3*n)*u;
sys = [xs(:);P(:)];
%%%%%%%%%%%
% Outputs %
%%%%%%%%%%%
ca 3,
服务的近义词
xs = x(1:3*n,1);
P = zeros(3*n,3*n);
P(:) = x(3*n+1:end,1);
% Measurement update
y = z(1);
u = z(2);
R = 1;
H = [1 zeros(1,n-1+2*n)];
K = P*H'/(H*P*H'+R); % only SISO models supported
xs = xs + K*(y-H*xs);
sys = x(1:3*n,:);
%%%%%%%%%%%%%%%%%%%%%%%
% GetTimeOfNextVarHit %
%%%%%%%%%%%%%%%%%%%%%%%
ca 4,
sys = [];
%%%%%%%%%%%%%%%%%%%%
% Unexpected flags %
%%%%%%%%%%%%%%%%%%%%
otherwi
sys = [];
end
% THIS PROGRAM IS FOR IMPLEMENTATION OF DISCRETE TIME PROCESS EXTENDED KALMAN FILTER
% FOR GAUSSIAN AND LINEAR STOCHASTIC DIFFERENCE EQUATION.郭沫若凤凰涅盘
