Hello everyone, I’m currently trying to nest and call the fmincon function within a MATLAB Function block in Simulink. However, I’ve encountered several errors. Below, I’ll explain my code in detail. The first 53 lines involve calculating various Jacobian matrices such as G, H, and D_asterisk. Once these calculations are complete, I use these matrices along with predefined functions to compute balambda. If balambda <= 1, I define a matrix k as k = eye(7). If balambda > 1, I need to optimize and solve for matrix k using the fmincon function. The initial value for k is set as k = diag([ones(1,4) lambda(1,1) lambda(2,2) lambda(3,3)]). Thus, I need to optimize lambda using fmincon, where k is implicit in the objective function. To obtain my objective function, a series of intermediate variables such as p_, k_gain, and xr need to be derived from k matrix. Currently, my approach is resulting in errors. How can I achieve my goal? I understand fmincon cannot be called directly and requires setting up a wrapper. How should I set up this wrapper?
function [x,xr,G1,G2,G,H,D_asterisk,V,balambda]=AEKF_UI(x,Ts,ks,kt,cs,mb,mw,y,xr_asterisk,G10,G20)
% x is the input vector
% Define the nonlinear function f(x)
% Ts是离散时间
% 单位矩阵定义得有2个，因为状态维数和观测维数不一致
Q=diag([0.1^2,0.005^2,0.1^2,0.001^2 0.1^2,0.005^2,0.1^2]);%
R=diag([0.001^2,0.001^2]);
g=@(x,xr_asterisk)[x(2);cs/mb*(x(4)-x(2))+ks/mb*(x(3)-x(1));
x(4);cs/mw*(x(2)-x(4))+ks/mw*(x(1)-x(3))+kt/mw*xr_asterisk-kt/mw*x(3);0;0;0;];
h=@(x,xr_asterisk)[cs/mb*(x(4)-x(2))+ks/mb*(x(3)-x(1));cs/mw*(x(2)-x(4))+ks/mw*(x(1)-x(3))+kt/mw*xr_asterisk-kt/mw*x(3)];
% Number of variables
n=numel(x);
p=eye(n);
f=numel(xr_asterisk);
% Number of functions
m=numel(g(x,xr_asterisk));%numel用来计算元素的个数，3*4矩阵元素共有12个
l=numel(h(x,xr_asterisk));
I1=eye(n);
I2=eye(l);
% Initialize the Jacobian matrix
G=zeros(m,n);
H=zeros(l,n);
D_asterisk=zeros(l,f);
%D矩阵要对未知输入求偏导
% Small perturbation value for finite difference
delta=1e-6;
% Compute the Jacobian matrix using central difference approximation
x_=x+Ts*g(x,xr_asterisk);
% 对x求偏导
for i=1:n
x1=x;
x2=x;
x3=x_;
x4=x_;
x1(i)=x1(i)+delta;
x2(i)=x2(i)-delta;
x3(i)=x3(i)+delta;
x4(i)=x4(i)-delta;
g1=g(x1,xr_asterisk);
g2=g(x2,xr_asterisk);
h1=h(x3,xr_asterisk);
h2=h(x4,xr_asterisk);
% Finite difference approximation
G(:,i)=(g1-g2)/(2*delta);
H(:,i)=(h1-h2)/(2*delta);
end
for i=1:f
x5=xr_asterisk;
x6=xr_asterisk;
x5(i)=x5(i)+delta;
x6(i)=x6(i)-delta;
D_asterisk(:,i)=(h(x_,x5)-h(x_,x6))/(2*delta);
end
gamma=y-h(x_,xr_asterisk);
mu=0.95;
persistent balambda
if isempty(balambda)
balambda=0.999;
end
G1=gamma*gamma’+mu*(G10/balambda);
G2=1+mu*(G20/balambda);
V=G1/G2;
T1=H*(I1+Ts*G)*p*(I1+Ts*G)’*H’;
T2=H*Q*H’+R;
Ta=trace(T1*inv(R)*T1′);
Tb=trace(T1*inv(R)*T2’+T2*inv(R)*T1′);
Tc=trace(T2*inv(R)*T2′)-trace(V)
balambda=(-Tb+sqrt(Tb^2-4*Ta*Tc))/(2*Ta);
if balambda>1
k=eye(7);%lambda矩阵
else
lambda=diag([balambda*ones(1,3)]);
k=diag([ones(1,4) lambda(1,1) lambda(2,2) lambda(3,3)]);
%定义目标函数
%创建一个自适应因子对角矩阵，前面状态量都是1，后面识别参数都是lambda
initial_values=[lambda(1,1),lambda(2,2),lambda(3,3)];
x_chushi=x;
lb=[1,1,1]; % 下界
ub=[Inf,Inf,Inf]; % 上界
options=optimoptions(‘fmincon’,’Display’,’iter’);
lambda_opt=fmincon(@(lambda)objective_function(lambda,x_chushi,I1,I2,Ts,G,D_asterisk,R,h,x_,xr_asterisk,Q,p,H,y),initial_values,[],[],[],[],lb,ub,@(lambda)nonlcon(V,R,H,p_),options);
%更新lambda 值
lambda(1,1)=lambda_opt(1);
lambda(2,2)=lambda_opt(2);
lambda(3,3)=lambda_opt(3);
end
function J_value=objective_function(lambda,x_chushi,I1,I2,Ts,G,D_asterisk,R,h,x_,xr_asterisk,Q,p,H,y,k)
% objective_function k=diag([ones(1,4) lambda(1) lambda(2) lambda(3)]);
p_=k*(I1+Ts*G)*p*(I1+Ts*G)’*k’+Q;
k_gain=p_*H’/(H*p_*H’+R);
S=inv(D_asterisk’*inv(R)*(I2-H*k_gain)*D_asterisk);
xr=S*(D_asterisk’)*inv(R)*(I2-H*k_gain)*(y-h(x_,xr_asterisk)+D_asterisk*xr_asterisk);
x=x_+k_gain*(y-h(x_,xr_asterisk)-D_asterisk*(xr-xr_asterisk));
J_value=sum(abs((x(4:7)-x_chushi(4:7))./x_chushi(4:7)));
end
function [c,ceq] = nonlcon(V,R,H,p_)
% 非线性约束函数，计算状态量与观测量之间的差异
c=norm(V-(H*p_*H’+R)*inv(R)*(H*p_*H’+R)’,"fro")-0.01;
ceq=[];Hello everyone, I’m currently trying to nest and call the fmincon function within a MATLAB Function block in Simulink. However, I’ve encountered several errors. Below, I’ll explain my code in detail. The first 53 lines involve calculating various Jacobian matrices such as G, H, and D_asterisk. Once these calculations are complete, I use these matrices along with predefined functions to compute balambda. If balambda <= 1, I define a matrix k as k = eye(7). If balambda > 1, I need to optimize and solve for matrix k using the fmincon function. The initial value for k is set as k = diag([ones(1,4) lambda(1,1) lambda(2,2) lambda(3,3)]). Thus, I need to optimize lambda using fmincon, where k is implicit in the objective function. To obtain my objective function, a series of intermediate variables such as p_, k_gain, and xr need to be derived from k matrix. Currently, my approach is resulting in errors. How can I achieve my goal? I understand fmincon cannot be called directly and requires setting up a wrapper. How should I set up this wrapper?
function [x,xr,G1,G2,G,H,D_asterisk,V,balambda]=AEKF_UI(x,Ts,ks,kt,cs,mb,mw,y,xr_asterisk,G10,G20)
% x is the input vector
% Define the nonlinear function f(x)
% Ts是离散时间
% 单位矩阵定义得有2个，因为状态维数和观测维数不一致
Q=diag([0.1^2,0.005^2,0.1^2,0.001^2 0.1^2,0.005^2,0.1^2]);%
R=diag([0.001^2,0.001^2]);
g=@(x,xr_asterisk)[x(2);cs/mb*(x(4)-x(2))+ks/mb*(x(3)-x(1));
x(4);cs/mw*(x(2)-x(4))+ks/mw*(x(1)-x(3))+kt/mw*xr_asterisk-kt/mw*x(3);0;0;0;];
h=@(x,xr_asterisk)[cs/mb*(x(4)-x(2))+ks/mb*(x(3)-x(1));cs/mw*(x(2)-x(4))+ks/mw*(x(1)-x(3))+kt/mw*xr_asterisk-kt/mw*x(3)];
% Number of variables
n=numel(x);
p=eye(n);
f=numel(xr_asterisk);
% Number of functions
m=numel(g(x,xr_asterisk));%numel用来计算元素的个数，3*4矩阵元素共有12个
l=numel(h(x,xr_asterisk));
I1=eye(n);
I2=eye(l);
% Initialize the Jacobian matrix
G=zeros(m,n);
H=zeros(l,n);
D_asterisk=zeros(l,f);
%D矩阵要对未知输入求偏导
% Small perturbation value for finite difference
delta=1e-6;
% Compute the Jacobian matrix using central difference approximation
x_=x+Ts*g(x,xr_asterisk);
% 对x求偏导
for i=1:n
x1=x;
x2=x;
x3=x_;
x4=x_;
x1(i)=x1(i)+delta;
x2(i)=x2(i)-delta;
x3(i)=x3(i)+delta;
x4(i)=x4(i)-delta;
g1=g(x1,xr_asterisk);
g2=g(x2,xr_asterisk);
h1=h(x3,xr_asterisk);
h2=h(x4,xr_asterisk);
% Finite difference approximation
G(:,i)=(g1-g2)/(2*delta);
H(:,i)=(h1-h2)/(2*delta);
end
for i=1:f
x5=xr_asterisk;
x6=xr_asterisk;
x5(i)=x5(i)+delta;
x6(i)=x6(i)-delta;
D_asterisk(:,i)=(h(x_,x5)-h(x_,x6))/(2*delta);
end
gamma=y-h(x_,xr_asterisk);
mu=0.95;
persistent balambda
if isempty(balambda)
balambda=0.999;
end
G1=gamma*gamma’+mu*(G10/balambda);
G2=1+mu*(G20/balambda);
V=G1/G2;
T1=H*(I1+Ts*G)*p*(I1+Ts*G)’*H’;
T2=H*Q*H’+R;
Ta=trace(T1*inv(R)*T1′);
Tb=trace(T1*inv(R)*T2’+T2*inv(R)*T1′);
Tc=trace(T2*inv(R)*T2′)-trace(V)
balambda=(-Tb+sqrt(Tb^2-4*Ta*Tc))/(2*Ta);
if balambda>1
k=eye(7);%lambda矩阵
else
lambda=diag([balambda*ones(1,3)]);
k=diag([ones(1,4) lambda(1,1) lambda(2,2) lambda(3,3)]);
%定义目标函数
%创建一个自适应因子对角矩阵，前面状态量都是1，后面识别参数都是lambda
initial_values=[lambda(1,1),lambda(2,2),lambda(3,3)];
x_chushi=x;
lb=[1,1,1]; % 下界
ub=[Inf,Inf,Inf]; % 上界
options=optimoptions(‘fmincon’,’Display’,’iter’);
lambda_opt=fmincon(@(lambda)objective_function(lambda,x_chushi,I1,I2,Ts,G,D_asterisk,R,h,x_,xr_asterisk,Q,p,H,y),initial_values,[],[],[],[],lb,ub,@(lambda)nonlcon(V,R,H,p_),options);
%更新lambda 值
lambda(1,1)=lambda_opt(1);
lambda(2,2)=lambda_opt(2);
lambda(3,3)=lambda_opt(3);
end
function J_value=objective_function(lambda,x_chushi,I1,I2,Ts,G,D_asterisk,R,h,x_,xr_asterisk,Q,p,H,y,k)
% objective_function k=diag([ones(1,4) lambda(1) lambda(2) lambda(3)]);
p_=k*(I1+Ts*G)*p*(I1+Ts*G)’*k’+Q;
k_gain=p_*H’/(H*p_*H’+R);
S=inv(D_asterisk’*inv(R)*(I2-H*k_gain)*D_asterisk);
xr=S*(D_asterisk’)*inv(R)*(I2-H*k_gain)*(y-h(x_,xr_asterisk)+D_asterisk*xr_asterisk);
x=x_+k_gain*(y-h(x_,xr_asterisk)-D_asterisk*(xr-xr_asterisk));
J_value=sum(abs((x(4:7)-x_chushi(4:7))./x_chushi(4:7)));
end
function [c,ceq] = nonlcon(V,R,H,p_)
% 非线性约束函数，计算状态量与观测量之间的差异
c=norm(V-(H*p_*H’+R)*inv(R)*(H*p_*H’+R)’,"fro")-0.01;
ceq=[]; Hello everyone, I’m currently trying to nest and call the fmincon function within a MATLAB Function block in Simulink. However, I’ve encountered several errors. Below, I’ll explain my code in detail. The first 53 lines involve calculating various Jacobian matrices such as G, H, and D_asterisk. Once these calculations are complete, I use these matrices along with predefined functions to compute balambda. If balambda <= 1, I define a matrix k as k = eye(7). If balambda > 1, I need to optimize and solve for matrix k using the fmincon function. The initial value for k is set as k = diag([ones(1,4) lambda(1,1) lambda(2,2) lambda(3,3)]). Thus, I need to optimize lambda using fmincon, where k is implicit in the objective function. To obtain my objective function, a series of intermediate variables such as p_, k_gain, and xr need to be derived from k matrix. Currently, my approach is resulting in errors. How can I achieve my goal? I understand fmincon cannot be called directly and requires setting up a wrapper. How should I set up this wrapper?
function [x,xr,G1,G2,G,H,D_asterisk,V,balambda]=AEKF_UI(x,Ts,ks,kt,cs,mb,mw,y,xr_asterisk,G10,G20)
% x is the input vector
% Define the nonlinear function f(x)
% Ts是离散时间
% 单位矩阵定义得有2个，因为状态维数和观测维数不一致
Q=diag([0.1^2,0.005^2,0.1^2,0.001^2 0.1^2,0.005^2,0.1^2]);%
R=diag([0.001^2,0.001^2]);
g=@(x,xr_asterisk)[x(2);cs/mb*(x(4)-x(2))+ks/mb*(x(3)-x(1));
x(4);cs/mw*(x(2)-x(4))+ks/mw*(x(1)-x(3))+kt/mw*xr_asterisk-kt/mw*x(3);0;0;0;];
h=@(x,xr_asterisk)[cs/mb*(x(4)-x(2))+ks/mb*(x(3)-x(1));cs/mw*(x(2)-x(4))+ks/mw*(x(1)-x(3))+kt/mw*xr_asterisk-kt/mw*x(3)];
% Number of variables
n=numel(x);
p=eye(n);
f=numel(xr_asterisk);
% Number of functions
m=numel(g(x,xr_asterisk));%numel用来计算元素的个数，3*4矩阵元素共有12个
l=numel(h(x,xr_asterisk));
I1=eye(n);
I2=eye(l);
% Initialize the Jacobian matrix
G=zeros(m,n);
H=zeros(l,n);
D_asterisk=zeros(l,f);
%D矩阵要对未知输入求偏导
% Small perturbation value for finite difference
delta=1e-6;
% Compute the Jacobian matrix using central difference approximation
x_=x+Ts*g(x,xr_asterisk);
% 对x求偏导
for i=1:n
x1=x;
x2=x;
x3=x_;
x4=x_;
x1(i)=x1(i)+delta;
x2(i)=x2(i)-delta;
x3(i)=x3(i)+delta;
x4(i)=x4(i)-delta;
g1=g(x1,xr_asterisk);
g2=g(x2,xr_asterisk);
h1=h(x3,xr_asterisk);
h2=h(x4,xr_asterisk);
% Finite difference approximation
G(:,i)=(g1-g2)/(2*delta);
H(:,i)=(h1-h2)/(2*delta);
end
for i=1:f
x5=xr_asterisk;
x6=xr_asterisk;
x5(i)=x5(i)+delta;
x6(i)=x6(i)-delta;
D_asterisk(:,i)=(h(x_,x5)-h(x_,x6))/(2*delta);
end
gamma=y-h(x_,xr_asterisk);
mu=0.95;
persistent balambda
if isempty(balambda)
balambda=0.999;
end
G1=gamma*gamma’+mu*(G10/balambda);
G2=1+mu*(G20/balambda);
V=G1/G2;
T1=H*(I1+Ts*G)*p*(I1+Ts*G)’*H’;
T2=H*Q*H’+R;
Ta=trace(T1*inv(R)*T1′);
Tb=trace(T1*inv(R)*T2’+T2*inv(R)*T1′);
Tc=trace(T2*inv(R)*T2′)-trace(V)
balambda=(-Tb+sqrt(Tb^2-4*Ta*Tc))/(2*Ta);
if balambda>1
k=eye(7);%lambda矩阵
else
lambda=diag([balambda*ones(1,3)]);
k=diag([ones(1,4) lambda(1,1) lambda(2,2) lambda(3,3)]);
%定义目标函数
%创建一个自适应因子对角矩阵，前面状态量都是1，后面识别参数都是lambda
initial_values=[lambda(1,1),lambda(2,2),lambda(3,3)];
x_chushi=x;
lb=[1,1,1]; % 下界
ub=[Inf,Inf,Inf]; % 上界
options=optimoptions(‘fmincon’,’Display’,’iter’);
lambda_opt=fmincon(@(lambda)objective_function(lambda,x_chushi,I1,I2,Ts,G,D_asterisk,R,h,x_,xr_asterisk,Q,p,H,y),initial_values,[],[],[],[],lb,ub,@(lambda)nonlcon(V,R,H,p_),options);
%更新lambda 值
lambda(1,1)=lambda_opt(1);
lambda(2,2)=lambda_opt(2);
lambda(3,3)=lambda_opt(3);
end
function J_value=objective_function(lambda,x_chushi,I1,I2,Ts,G,D_asterisk,R,h,x_,xr_asterisk,Q,p,H,y,k)
% objective_function k=diag([ones(1,4) lambda(1) lambda(2) lambda(3)]);
p_=k*(I1+Ts*G)*p*(I1+Ts*G)’*k’+Q;
k_gain=p_*H’/(H*p_*H’+R);
S=inv(D_asterisk’*inv(R)*(I2-H*k_gain)*D_asterisk);
xr=S*(D_asterisk’)*inv(R)*(I2-H*k_gain)*(y-h(x_,xr_asterisk)+D_asterisk*xr_asterisk);
x=x_+k_gain*(y-h(x_,xr_asterisk)-D_asterisk*(xr-xr_asterisk));
J_value=sum(abs((x(4:7)-x_chushi(4:7))./x_chushi(4:7)));
end
function [c,ceq] = nonlcon(V,R,H,p_)
% 非线性约束函数，计算状态量与观测量之间的差异
c=norm(V-(H*p_*H’+R)*inv(R)*(H*p_*H’+R)’,"fro")-0.01;
ceq=[]; fmincon, optimization MATLAB Answers — New Questions

​