EFK的M函数,在simulink仿真的时候出现Inner matrix dimensions must agree出错

浏览:56889
function [sys,x0,str,ts,simStateCompliance] = sekf(t,x,u,flag)
switch flag,
case 0,
[sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes;
case 2,
sys=mdlUpdate(t,x,u);
case 3,
sys=mdlOutputs(t,x,u);

case{1,4,9},
sys=[];
otherwise
DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));
end
function [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates = 0;
sizes.NumDiscStates = 2;
sizes.NumOutputs = 1;
sizes.NumInputs = 2;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1; % at least one sample time is needed
sys = simsizes(sizes);
x0 = [0.8 0];
str = [];
ts = [1 0];
simStateCompliance = 'UnknownSimState';
function sys=mdlUpdate(t,x,u)
A=[1 0,0 1-1/5000];
B=[-1/50,1/1200];
Q=[0.08 0,0 0.08];%过程噪声协方差
R=0.0015; %观测噪声协方差
P0=eye(2);
%状态预测
Xefk=A*x+B*u(2);
%观测预测
Zd=1.761*Xefk(1,1)^3-2.167*Xefk(1,1)^2+1.485*Xefk(1,1)-Xefk(2,1)-0.02*u(2);
%线性化观测矩阵H
H=[3.532*Xefk(1,1)^2-4.334*Xefk(1,1)+1.485*Xefk(1,1),-1];
%协方差预测
P=A*P0*A'+Q;
%求Kalman增益
K=P*H'*inv(H*P*H'+R);
%状态更新
Xefk=Xefk+K*(u(1)-Zd);
%协方差更新
P0=(eye(2)-K*H)*P;
sys = [];
function sys=mdlOutputs(t,x,u)
sys =x(1);
邀请回答 我来回答

当前暂无回答

回答可获赠 200金币

没解决?试试专家一对一服务

换一批
    App下载
    技术邻APP
    工程师必备
    • 项目客服
    • 培训客服
    • 平台客服

    TOP