扩展卡尔曼滤波器的s函数编写?
来源:学生作业帮 编辑:作业帮 分类:综合作业 时间:2024/10/06 17:07:47
扩展卡尔曼滤波器的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.
哪里错了?
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;这里就出现了矩阵维数不对.你换过来就行了.
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;这里就出现了矩阵维数不对.你换过来就行了.
求扩展卡尔曼滤波器的英文论文 最好只讲算法
卡尔曼滤波器属于高通低通还是带通?
用matlab编写使用二维高斯低通滤波器的函数,使用大小为3*3,标准差分别为0.5,1.5,2.5
就是关于巴特沃斯滤波器的程序的编写.
请教一下matlab的fir滤波器设计,已知频率响应的表达式,要如何编写代码设计滤波器呢?
卡尔曼滤波算法的功能是什么?
【Matlab】Matlab中如何编写S函数
编写求阶乘的函数,并通过调用函数计算s=6!+10!+22!
用窗函数法设计FIR滤波器的主要特点是什么?
如何用matlab绘制FIR滤波器的损耗函数
怎样利用MATLAB画FIR滤波器的幅度函数?
小波分解概念上的基本关系 ,低通分解滤波器,高通分解滤波器,尺度函数,小波函数