How to nest and call the fmincon function in Simulink?

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)+3;
    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=diag(1,1,1]); % 下界
    ub=diag([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,V),initial_values,[],[],[],[],lb,ub,@(lambda)nonlcon(lambda,x_chushi,I1,I2,Ts,G,D_asterisk,R,h,x_,xr_asterisk,Q,p,H,y,V),options);
    %更新lambda 值
    lambda(1,1)=lambda_opt(1,1);
    lambda(2,2)=lambda_opt(2,2);
    lambda(3,3)=lambda_opt(3,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,V)
    % objective_function k=diag([ones(1,4) lambda(1) lambda(2) lambda(3)]);
    k=diag([ones(1,4) lambda(1,1) lambda(2,2) lambda(3,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(lambda,x_chushi,I1,I2,Ts,G,D_asterisk,R,h,x_,xr_asterisk,Q,p,H,y,V)
    % 非线性约束函数，计算状态量与观测量之间的差异
    k=diag([ones(1,4) lambda(1,1) lambda(2,2) lambda(3,3)]);
    p_=k*(I1+Ts*G)*p*(I1+Ts*G)'*k'+Q;
    c=norm(V-(H*p_*H'+R)*inv(R)*(H*p_*H'+R)',"fro")-0.01;
    ceq=[];

I first debugged the program in MATLAB, with the following initialization:

EKF_UI([20; 0; 10; 0; 29114; 233350; 925.8], 0.001, 29114, 233350, 925.8, 240, 56.9, [0.1; 0.2], 0, 0, 0)

However, I encountered the following error:

[Aquatris]

What error do you actually get? without that info it is hard to say anything

[guo qing]

Thank you for your response. I have updated my question.

Answer 1

[Aquatris]

EDITED: to fix the initial_values to be a 3x3 matrix instead of lambda

Your issue was in the line

initial_values=[lambda(1,1),lambda(2,2),lambda(3,3)];

This does not create a 3x3 matrix as used in your objective function. SO you should use

initial_values=diag([lambda(1,1),lambda(2,2),lambda(3,3)]);

Once you fix this, then your second problem comes. fmincon does not work with imaginary values. So have a look at this answer to a similar case and you should be able to adapt it to your problem. The imaginary parts comes from the line where you find the roots of a 2nd order polynomial in this part:

balambda=(-Tb+sqrt(Tb^2-4*Ta*Tc))/(2*Ta)+3;

[guo qing]

Thank you for your response. So, the error "Barrier Objective function is undefined at initial point. Fmincon cannot continue." is due to the occurrence of complex values. I would also like to ask if the parameters passed to the objective function and nonlinear constraint function (nonclon) need to be exactly the same. When there is an inconsistency, does it cause parameter passing errors?

[Aquatris]

So, the error "Barrier Objective function is undefined at initial point. Fmincon cannot continue." is due to the occurrence of complex values.

No this was because your initial_value variable was 1x3 matrix whereas your objective function required 3x3 matrix. I had an error in explaining it so fixed that part.

You defined the initial_value variable as

initial_values=[lambda(1,1),lambda(2,2),lambda(3,3)];

and in the objective function you try to reach lambda(2,2) which initially will try to reach initial_value(2,2). This is the error.

k=diag([ones(1,4) lambda(1,1) lambda(2,2) lambda(3,3)]);

I would also like to ask if the parameters passed to the objective function and nonlinear constraint function (nonclon) need to be exactly the same.

The parameters that are not being optimized do not need to be the same. So you can have objective_function that requires argument x,y and z where x is the parameters to be optimized and your constraint might only require x.

[guo qing]

Thank you for your response. In fact, if I directly set balambda = 3, I also encounter the error "Barrier Objective function is undefined at initial point. Fmincon cannot continue." Why does this happen?And I know that I need to set up a wrapper function in Simulink, but currently, I am not sure how to set it up for my function. Could you provide some good suggestions?

[Aquatris]

Cause as I explained, the cause of the error is not imaginary numbers. The cause is the initial_values variable you supply to the fmincon is a 1x3 matrix. Unless you change that, that error will stay. So replace this line

initial_values=[lambda(1,1),lambda(2,2),lambda(3,3)];

with

initial_values= lambda;

or

initial_values =

initial_values=diag([lambda(1,1),lambda(2,2),lambda(3,3)]);

[guo qing]

Thank you, but I have actually changed the initial values,

initial_values=diag([lambda(1,1),lambda(2,2),lambda(3,3)]);

and this issue still occurs.

function [x,xr,G1,G2]=EKF_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.0001^2,0.0001^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)];
    % numel函数计算元素个数
    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
    %自适应因子lambda的确定
    gamma=y-h(x_,xr_asterisk);
    mu=0.95;
    persistent balambda
    if isempty(balambda)
    balambda=0.999;
    end
    G1=gamma*gamma'+mu*(G10/balambda);
    dbstop if error
    G2=1+mu*(G20/balambda);  
    V=G1/G2; 
    %若数据未发生突变，则缓慢遗忘，若发生突变则快速遗忘
    %用求迹的方法，实际上对自适应因子矩阵进行了假设，假设他们都相等，
    %这一步只是为了得到lambda的一个合理初值，并不是最后的lambda矩阵
    %最后的lambda矩阵仍需要通过约束优化方法求解    
    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)
    if Tb^2-4*Ta*Tc>0
        disp(Tb^2-4*Ta*Tc)
    else 
        disp(Tb^2-4*Ta*Tc)
    end
    balambda=(-Tb+sqrt(Tb^2-4*Tc))/(2*Ta)+3
    balambda
    lambda=[balambda*eye(3)]
    beta=lambda;
    if balambda<=1
    k=eye(7);%lambda矩阵
    else
    %定义目标函数
    %创建一个自适应因子对角矩阵，前面状态量都是1，后面识别参数都是lambda
    x_chushi=x;
    k=diag([ones(1,4) lambda(1,1) lambda(2,2) lambda(3,3)]);
    initial_values=diag([lambda(1,1),lambda(2,2),lambda(3,3)]); 
    lb=diag([1,1,1]); % 下界
    ub=diag([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,V),initial_values,[],[],[],[],lb,ub,@(lambda)nonlcon(lambda,I1,Ts,G,R,Q,p,H,V),options);
    %更新lambda 值
    lambda(1,1)=lambda_opt(1,1);
    lambda(2,2)=lambda_opt(2,2);
    lambda(3,3)=lambda_opt(3,3);
    end
    %未知输入估计
end
    function J_value=objective_function(lambda,x_chushi,I1,I2,Ts,G,D_asterisk,R,h,x_,xr_asterisk,Q,p,H,y,V)
    % 目标函数k=diag([ones(1,4) lambda(1) lambda(2) lambda(3)]);
    k=diag([ones(1,4) lambda(1,1) lambda(2,2) lambda(3,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(lambda,I1,Ts,G,R,Q,p,H,V)
    % 非线性约束函数，计算状态量与观测量之间的差异
    k=diag([ones(1,4) lambda(1,1) lambda(2,2) lambda(3,3)]);
    p_=k*(I1+Ts*G)*p*(I1+Ts*G)'*k'+Q;
    c=norm(V-(H*p_*H'+R)*inv(R)*(H*p_*H'+R)',"fro")-0.01;
    ceq=[];
    end

[Aquatris]

There are multiple different causes depending on the other variables you send to the function. One of them is your objective function returns 'inf' when x_chushi has 0 in it, which depends on many things in your code so it is not easy for someone that has no clue what problem you are solving.

What I recommend is put a debug point at the fmincon() line and the first line in objective_function, call the EKF_UI() function with the parameters, and stepin the objective function to see what is actually happening.

So here for instance I give a brief example showing objective function is inf which should not happen for optimization problems:

EKF_UI([20; 0; 10; 0; 29114; 233350; 925.8], 0.001, 29114, 233350, 925.8, 240, 56.9, [0.1; 0.2], 0, 0, 0);

objectiveFunctionVal = Inf

function [x,xr,G1,G2,G,H,D_asterisk,V,balambda]=EKF_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;
    balambda=0.999;

    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)+3;
    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 = eye(3); %% CHANGE INITIAL VALUES
    x_chushi=x;
    
    %% CALL OBJ FUNCTION ONLY
    objectiveFunctionVal = objective_function(initial_values,x_chushi,I1,I2,Ts,G,D_asterisk,R,h,x_,xr_asterisk,Q,p,H,y,V)
end
    function J_value=objective_function(lambda,x_chushi,I1,I2,Ts,G,D_asterisk,R,h,x_,xr_asterisk,Q,p,H,y,V)
    % objective_function k=diag([ones(1,4) lambda(1) lambda(2) lambda(3)]);
    k=diag([ones(1,4) lambda(1,1) lambda(2,2) lambda(3,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(lambda,x_chushi,I1,I2,Ts,G,D_asterisk,R,h,x_,xr_asterisk,Q,p,H,y,V)
    % 非线性约束函数，计算状态量与观测量之间的差异
    k=diag([ones(1,4) lambda(1,1) lambda(2,2) lambda(3,3)]);
    p_=k*(I1+Ts*G)*p*(I1+Ts*G)'*k'+Q;
    c=norm(V-(H*p_*H'+R)*inv(R)*(H*p_*H'+R)',"fro")-0.01;
    ceq=[];
    end
end

[guo qing]

Thank you very much. I found that as long as ( x_{\text{chushi}} ) is set to non-zero values from the 4th to the 7th components, the issue is resolved. However, I've encountered another problem: during the optimization process, I noticed that three variables in the lambda matrix I want to optimize are equal. Why is this happening? Also, how should I set up the fmincon function in Simulink?