How to nest and call the fmincon function in Simulink?
3 visualizaciones (últimos 30 días)
Mostrar comentarios más antiguos
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:
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1719116/image.png)
2 comentarios
Aquatris
el 20 de Jun. de 2024
What error do you actually get? without that info it is hard to say anything
Respuestas (1)
Aquatris
el 20 de Jun. de 2024
Editada: Aquatris
el 20 de Jun. de 2024
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;
7 comentarios
Aquatris
el 20 de Jun. de 2024
Editada: Aquatris
el 20 de Jun. de 2024
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);
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
Ver también
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!