cvx_begin quiet
cvx_precision high %定义优化为高精度
variable q1(2,1) %定义了两个优化变量
variable q2(2,1)
expression x
…
subject to
pmax=2000;
p0=580.65;
p_ind=790.671;
U_tip=200;
v0=4.03;
d0=0.3;
tho=1.225;
s=0.05;
A=0.79;
v1=norm([(q1(1)-q1_loc(1,n-1)),(q1(2)-q1_loc(2,n-1))])*inv_pos(deta_T);
v2=norm([(q2(1)-q2_loc(1,n-1)),(q2(2)-q2_loc(2,n-1))])*inv_pos(deta_T);
% v1=sqrt((q1(1)-q1_loc(1,n-1))^2+(q1(2)-q1_loc(2,n-1))^2)*inv_pos(deta_T);
% v2=sqrt((q2(1)-q2_loc(1,n-1))^2+(q2(2)-q2_loc(2,n-1))^2)inv_pos(deta_T);
%x=pow_pos(v1,2)inv_pos(2v0^2);
x=pow_pos((v1inv_pos(sqrt(2)*v0)),2);
s1_2=norm([1,x])-x;
Apparently, x is a convex expression and is not affine. (Note: you haven’t shown us what x is.) The argument of `norm must be affine.
I don’t know whether as reformulation consistent with CVX’s rules exists, because I don’t know what x is, and I don’t know how you intend to use s1_2.
Your first task is to prove your optimization problem is convex.
Mark,thank you very much for your reply. Next, I will rephrase my question more precisely.
“x=pow_pos((v1inv_pos(sqrt(2)v0)),2);"
While ‘x’ is the expression above, which is equivalent to replacing this expression with ‘x’ to make the norm formula more concise; ‘s1_2’ is a parameter introduced to turn this constraint( P_1=p0(1+3v1^2/(U_tip^2))+p_indsqrt(s1_2)+0.5d0thosAv1^2*inv_pos(deta_T);)into a convex constraint, and later, a first-order Taylor expansion will be applied to ‘s1_2’ to also make it a convex constraint. However, due to the current error, I am unable to process the subsequent formulas.
‘x’ is not affine, so if I want to use ‘norm’, what kind of processing can be done on ‘x’ to make it comply with CVX’s rules? I would be extremely grateful if I could receive your reply again.”
Do your convexification on norm(expression), so that CVX never is presented with norm of a non-affine expression. Perhaps your convexification will produce an expression which doesn’t even involve norm. How you convexify, or whether you convexify is up to you. There are tools for non-convex optimization, for instance under YALMIP.
Okay, thank you very much for your reply.
Do you mean that I should first convexify s1_2, turning it into a convex function, and then substitute it into the expression for P_1=p0*(1+3* v1^2/(U_tip^2))+p_indsqrt(s1_2)+0.5 d0tho sA v1^2*inv_pos(deta_T)
As an example, norm([x^2,1/y]) is equal to sqrt(x^4+1/y^2). So convexify the latter. Then CVX never sees norm of anything.
I am not telling you how to convexify. That is up to you. it’s your optimization problem, which you presumably understand; whereas I have no idea what you are doing.
Okay, I will try to work on it, thank you very much for your idea, which might be very helpful to me.
Hi,Mark! According to your suggestion, I have linearized the non-convex part of s1_2 and successfully resolved the “norm” issue. However, I am now encountering a new error as follow.
错误使用 +
Disciplined convex programming error:
Illegal operation: {convex} + {concave}
出错 UAV (第 204 行)
P_1=p0*(1+3pow_pos(v1,2)/(U_tip^2))+p_inds1+0.5d0thosA*pow_pos(v1,2)*inv_pos(deta_T);
pmax=2000;
p0=580.65;
p_ind=790.671;
U_tip=200;
v0=4.03;
d0=0.3;
tho=1.225;
s=0.05;
A=0.79;
v1=norm([(q1(1)-q1_loc(1,n-1)),(q1(2)-q1_loc(2,n-1))])*inv_pos(deta_T);
v2=norm([(q2(1)-q2_loc(1,n-1)),(q2(2)-q2_loc(2,n-1))])*inv_pos(deta_T);
% v1=sqrt((q1(1)-q1_loc(1,n-1))^2+(q1(2)-q1_loc(2,n-1))^2)*inv_pos(deta_T);
% v2=sqrt((q2(1)-q2_loc(1,n-1))^2+(q2(2)-q2_loc(2,n-1))^2)inv_pos(deta_T);
%x=pow_pos(v1,2)inv_pos(2v0^2);
% x=pow_pos((v1inv_pos(sqrt(2)*v0)),2);
% s1_2=norm([1,x])-x;
v1_k=sqrt((q1_r(1)-q1_loc(1,n-1))^2+(q1_r(2)-q1_loc(2,n-1))^2)*inv_pos(deta_T);
v2_k=sqrt((q2_r(1)-q2_loc(1,n-1))^2+(q2_r(2)-q2_loc(2,n-1))^2)*inv_pos(deta_T);
v11=v1_k^2*inv_pos(2*v0^2)+v1_k*(v1-v1_k)*inv_pos(v0^2);
s1_2=norm([1,v11])-pow_pos(v1,2)*inv_pos(2*v0^2);
%s1_2 = sqrt(1+v1^4/(4*v0^4))-v1^2/(2*v0^2);%s^2[n]
s1=sqrt(s1_2);
P_1=p0*(1+3*pow_pos(v1,2)/(U_tip^2))+p_ind*s1+0.5*d0*tho*s*A*pow_pos(v1,2)*inv_pos(deta_T);
How can I address the issue of adding a ‘convex’ and a ‘concave’ function? I look forward to your reply.
You address the issue by forming a convex optimization problem for CVX to solve. It appears your attempted convexification didn’t actually make things convex. the input data values can matter in determining convexity or concavity. For example the sign of a matters if y is a convex expression in a*y.
Hi，Mark!I am currently facing a new problem: it is still this piece of code, but when I solve it, I find that the optimal solution obtained by minimizing the objective function is getting larger and larger. I have tried many methods but have not been able to solve it. I hope to get your help. If you can reply to me, I would be very grateful. Here is the complete code.
clc;
clear;
close all;
%% System Basic Paramters
q1s = [140,100]‘; %无人机1的位置
q2s = [120,200]’; %无人机2的位置
% q2s = [130,290]‘;
% q2s = [180,370]’;
% q2s = [240,450]‘;
% q1s=[165,106]’;
% q2s=[146,185]';
dismin = 40; %m
hm = 50; %m
% N = 15; %迭代循环次数
N=15;
rmax = 6; %优化的步长
deta_T = 0.5; %时间间隔 s
v1max = 20; %无人机的最大速度 m/s
v2max = 20;
%% Communication Model
imag = sqrt(-1); %定义虚数
% mu = 2;
% epsilon = 0.5;
e1 = 25; %LOS概率模型中与环境有关的参数,plos的计算?
e2 = 0.112; %LOS概率模型中与环境有关的参数,plos的计算?
c = 310^8; %多普勒频移计算中的光速 m/s
a2 = 1.210^(-7); %和系统特性有关的常数，计算sigma_tau?
Sigma = 1e-11; %反射回波信号中的噪声功率，计算sigma_tau?
% Pa = 40; %发射功率 dBm 10w,计算信噪比和sigma_tau用到
Pa=10;
Nsym = 10^4; %系统增益,计算sigma_tau?
Nt = 16; %天线个数
Nr = 16;
Lamda = 0.01; %计算Beta_r?
Epsilon = 100; %计算Beta_r?
% Gamma = 11;
% a = sqrt(1/Nt) * exp(-imag * pi * cos (deg2rad(theta)) * (0:Nt-1)');
kk = 0.2; %信道增益，NLos信道的衰减效应，计算betad?
% beta0 = 1e-8;
% beta_d = beat0*(Plos+(1-Plos)kk);
% hc = sqrt(beta_dd1^-2)a;
% P1 = 10;
% noise_c = 1e-14;
% gamma = P1Ntbeta_d/(d1^2noise_c);
% gamma_c = 100;
Beta_r = (Lamda)^3Epsilon/(64pi^3);%复值反射系数,sigma_tau计算公式中的epsilon ?
beta0 = 1e-8; %参考距离为1时的信道功率 w -80dB(/-60dB)
noise_c = 1e-14; %接收噪声功率 w -140dB(/-110dB)
%% EKF parameters
%总共四行，每一列是各个时刻的x,y,vx,xy
%x_real = importdata(‘C:\Users\Lenovo\Desktop\text studio\simulation\x_real.xlsx’); %MT的真实位置，速度信息
x_real = importdata(‘D:\桌面\real.xlsx’); %MT的真实位置，速度信息
u_real = x_real([1, 2],:); %提取第一行和第二行所有列的元素,MT位置坐标
x_estimated = zeros(4, N+1); %用于存放预测的位置
xs = [100,100,10,10]'; %用于预测的初始位置和速度定义
x_estimated(:,1) = xs;
us = xs([1, 2],:); %用于预测时初始的位置坐标
G = [1,0,deta_T, 0;
0,1, 0, deta_T;
0,0, 1, 0;
0,0, 0, 1];
M = eye(4); %初始的协方差定义为1，4*4的单位矩阵
Q = diag([1,1,0.5,0.5]);
R0 = diag(1);
q1_loc = zeros(2,N+1); %存放无人机1的轨迹
q1_loc(:,1) = q1s; %第一列为无人机1的初始位置
q2_loc = zeros(2,N+1); %存放无人机2的轨迹
q2_loc(:,1) = q2s; %第一列为无人机2的初始位置
val = zeros(N,rmax); %一个数值0
%% Optimization
%for n = 1:N
for n=2:N
% EKF time update part
%MT的位置变化
%x_p = G * x_real(:,n); %状态预测
x_p = G * x_estimated(:,n-1); %状态预测
M_p = G * M * conj(G') + Q; %协方差
u_p = x_p([1, 2],:); %估计位置信息
q1_r = q1_loc(:,n-1); %无人机的轨迹
q2_r = q2_loc(:,n-1);%%
for r = 1:rmax
cvx_begin quiet
cvx_precision high %定义优化为高精度
variable q1(2,1) %定义了两个优化变量
variable q2(2,1)
expression P_1
expression P_2
% expression x
% Calculate objective function
% d1 = norm(u_p-q1_loc(:,n-1))^2+hm^2;
% d2 = norm(u_p-q2_loc(:,n-1))^2+hm^2;
d1 = norm(u_p-q1_r)^2+hm^2;
d2 = norm(u_p-q2_r)^2+hm^2;
yita1=[1,0]';
yita2=[0,1]';
Sigma_tau1 = a2^2*Sigma*d1^2*inv_pos(Pa*Nsym*Nt*Nr*Beta_r); %时延的测量噪声误差
Sigma_tau2 = a2^2*Sigma*d2^2*inv_pos(Pa*Nsym*Nt*Nr*Beta_r);
M_p_inv=inv(M_p(1:2,1:2));
% jaq1 = [2*(q1_r(1)-u_p(1))*inv_pos(c^2*Sigma_tau1*d1) , 0]'; %pa的偏导
% jaq2 = [2*(q2_r(1)-u_p(1))*inv_pos(c^2*Sigma_tau2*d2) , 0]';
%
% jbq1 = [0 , 2*(q1_r(2)-u_p(2))*inv_pos(c^2*Sigma_tau1*d1)]'; %pb的偏导
% jbq2 = [0 , 2*(q2_r(2)-u_p(2))*inv_pos(c^2*Sigma_tau2*d2)]';
%
% jcq1 = [(q1_r(2)-u_p(2))*inv_pos(c^2*Sigma_tau1*d1) ,(q1_r(1)-u_p(1))*inv_pos(c^2*Sigma_tau1*d1)]'; %pc的偏导
% jcq2 = [(q2_r(2)-u_p(2))*inv_pos(c^2*Sigma_tau2*d2) ,(q2_r(1)-u_p(1))*inv_pos(c^2*Sigma_tau2*d2)]';
jaq1 = [(2*(q1_r(1)-u_p(1))*inv_pos(c^2*Sigma_tau1*d1))-((6*(q1_r(1)-u_p(1))^3)*inv_pos(c^2*Sigma_tau1*d1*d1)) ; 0]; %pa的偏导
jaq2 = [(2*(q2_r(1)-u_p(1))*inv_pos(c^2*Sigma_tau2*d2))-((6*(q2_r(1)-u_p(1))^3)*inv_pos(c^2*Sigma_tau2*d2*d2)) ; 0];
jbq1 = [0 ; (2*(q1_r(2)-u_p(2))*inv_pos(c^2*Sigma_tau1*d1))-((6*(q1_r(2)-u_p(2))^3)*inv_pos(c^2*Sigma_tau1*d1*d1))]; %pb的偏导
jbq2 = [0 ; (2*(q2_r(2)-u_p(2))*inv_pos(c^2*Sigma_tau2*d2))-((6*(q2_r(2)-u_p(2))^3)*inv_pos(c^2*Sigma_tau2*d2*d2))];
jcq1 = [((q1_r(2)-u_p(2))*inv_pos(c^2*Sigma_tau1*d1))-((6*(q1_r(1)-u_p(1))^2*(q1_r(2)-u_p(2)))*inv_pos(c^2*Sigma_tau1*d1*d1)) ; ((q1_r(1)-u_p(1))*inv_pos(c^2*Sigma_tau1*d1))-((6*(q1_r(2)-u_p(2))^2*(q1_r(1)-u_p(1)))*inv_pos(c^2*Sigma_tau1*d1*d1))]; %pc的偏导
jcq2 = [((q2_r(2)-u_p(2))*inv_pos(c^2*Sigma_tau2*d2))-((6*(q2_r(1)-u_p(1))^2*(q2_r(2)-u_p(2)))*inv_pos(c^2*Sigma_tau2*d2*d2)) ; ((q2_r(1)-u_p(1))*inv_pos(c^2*Sigma_tau2*d2))-((6*(q2_r(2)-u_p(2))^2*(q2_r(1)-u_p(1)))*inv_pos(c^2*Sigma_tau2*d2*d2))];
% jaqr=((yita1'*(u_p-q1_loc(:,n)))^2)*inv_pos(c^2*Sigma_tau1*d1)+((yita1'*(u_p-q2_loc(:,n)))^2)*inv_pos(c^2*Sigma_tau2*d2);
% jbqr=((yita2'*(u_p-q1_loc(:,n)))^2)*inv_pos(c^2*Sigma_tau1*d1)+((yita2'*(u_p-q2_loc(:,n)))^2)*inv_pos(c^2*Sigma_tau2*d2);
% jcqr=(yita1'*(u_p-q1_loc(:,n))*yita2'*(u_p-q1_loc(:,n)))*inv_pos(c^2*Sigma_tau1*d1)+(yita1'*(u_p-q2_loc(:,n))*yita2'*(u_p-q2_loc(:,n)))*inv_pos(c^2*Sigma_tau2*d2);
%
jaqr=((yita1'*(u_p-q1_r))^2)*inv_pos(c^2*Sigma_tau1*d1)+((yita1'*(u_p-q2_r))^2)*inv_pos(c^2*Sigma_tau2*d2);
jbqr=((yita2'*(u_p-q1_r))^2)*inv_pos(c^2*Sigma_tau1*d1)+((yita2'*(u_p-q2_r))^2)*inv_pos(c^2*Sigma_tau2*d2);
jcqr=(yita1'*(u_p-q1_r)*yita2'*(u_p-q1_r))*inv_pos(c^2*Sigma_tau1*d1)+(yita1'*(u_p-q2_r)*yita2'*(u_p-q2_r))*inv_pos(c^2*Sigma_tau2*d2);
% jaqr=((yita1'*(u_p-q1))^2)*inv_pos(c^2*Sigma_tau1*d1)+((yita1'*(u_p-q2))^2)*inv_pos(c^2*Sigma_tau2*d2);
% jbqr=((yita2'*(u_p-q1))^2)*inv_pos(c^2*Sigma_tau1*d1)+((yita2'*(u_p-q2))^2)*inv_pos(c^2*Sigma_tau2*d2);
% jcqr=(yita1'*(u_p-q1)*yita2'*(u_p-q1))*inv_pos(c^2*Sigma_tau1*d1)+(yita1'*(u_p-q2)*yita2'*(u_p-q2))*inv_pos(c^2*Sigma_tau2*d2);
%
gradi_q1=(((jaqr+M_p_inv(1,1))*(jbqr+M_p_inv(2,2))-(jcqr+M_p_inv(1,2))*(jcqr+M_p_inv(2,1))-(jaqr+M_p_inv(1,1)+jbqr+M_p_inv(2,2))*(jbqr+M_p_inv(2,2)).*jaq1)+...
((jaqr+M_p_inv(1,1))*(jbqr+M_p_inv(2,2))-(jcqr+M_p_inv(1,2))*(jcqr+M_p_inv(2,1))-(jaqr+M_p_inv(1,1)+jbqr+M_p_inv(2,2))*(jaqr+M_p_inv(1,1)).*jbq1)+...
((jaqr+M_p_inv(1,1)+jbqr+M_p_inv(2,2))*(2*jcqr+M_p_inv(1,2)+M_p_inv(2,1)).*jcq1))*inv_pos(((jaqr+M_p_inv(1,1))*(jbqr+M_p_inv(2,2))-(jcqr+M_p_inv(1,2))*(jcqr+M_p_inv(2,1)))^2);
gradi_q2=(((jaqr+M_p_inv(1,1))*(jbqr+M_p_inv(2,2))-(jcqr+M_p_inv(1,2))*(jcqr+M_p_inv(2,1))-(jaqr+M_p_inv(1,1)+jbqr+M_p_inv(2,2))*(jbqr+M_p_inv(2,2)).*jaq2)+...
((jaqr+M_p_inv(1,1))*(jbqr+M_p_inv(2,2))-(jcqr+M_p_inv(1,2))*(jcqr+M_p_inv(2,1))-(jaqr+M_p_inv(1,1)+jbqr+M_p_inv(2,2))*(jaqr+M_p_inv(1,1)).*jbq2)+...
((jaqr+M_p_inv(1,1)+jbqr+M_p_inv(2,2))*(2*jcqr+M_p_inv(1,2)+M_p_inv(2,1)).*jcq2))*inv_pos(((jaqr+M_p_inv(1,1))*(jbqr+M_p_inv(2,2))-(jcqr+M_p_inv(1,2))*(jcqr+M_p_inv(2,1)))^2);
% gradi_q1 = ((jaqr*jbqr-jcqr^2).*(jaq1+jbq1)-... %求梯度，凸逼近
% (jaqr+jbqr)*(jbqr.*jaq1 + jaqr.*jbq1 - 2*jcqr.*jcq1))*inv_pos((jaqr*jbqr-jcqr^2)^2);
% % gradi_q1 = ((jaq1.*jbq1-jcq1.^2).*(jaq1+jbq1)-... %求梯度，凸逼近
% % (jaq1+jbq1).*(jbq1.*jaq1 + jaq1.*jbq1 - 2.*jcq1.*jcq1)).*inv_pos((jaq1.*jbq1-jcq1.^2).^2);
% gradi_q2 = ((jaqr*jbqr-jcqr^2).*(jaq2+jbq2)-...
% (jaqr+jbqr)*(jbqr.*jaq2 + jaqr.*jbq2 - 2*jcqr.*jcq2))*inv_pos((jaqr*jbqr-jcqr^2)^2);
% % gradi_q2 = ((jaq2.*jbq2-jcq2.^2).*(jaq2+jbq2)-...
% % (jaq2+jbq2).*(jbq2.*jaq2 + jaq2.*jbq2 - 2.*jcq2.*jcq2)).*inv_pos((jaq2.*jbq2-jcq2.^2).^2);
F = gradi_q1'*(q1-q1_r) + gradi_q2'*(q2-q2_r)+(jaqr+jbqr+M_p_inv(1,1)+M_p_inv(2,2))*inv_pos((jaqr+M_p_inv(1,1))*(jbqr+M_p_inv(2,2))-(jcqr+M_p_inv(1,2))*(jcqr+M_p_inv(2,1)));
% cvx_begin quiet
% cvx_precision high %定义优化为高精度
% variable q1(2,1) %定义了两个优化变量
% variable q2(2,1)
minimize (F)
% maximize(F)
subject to
% Channel error constraints (44a)
% e1 = 25;
% e2 = 0.112;
% d11 = sqrt(norm(u_p-q1_r)^2+hm^2+2*(q1_r(1)-u_p(1))*(q1(1)-q1_r(1))+2*(q1_r(2)-u_p(2))*(q1(2)-q1_r(2)));
% % d11=pow_pos((u_p(1)-q1(1)),2)+pow_pos((u_p(2)-q1(2)),2)+hm^2;
% theta = (180/pi) * hm*inv_pos(d11);
% % theta = (180/pi) * acos(hm .*inv_pos(power(d11,0.5)));
% %theta = atan((u_p(2)-q1_loc(2,n)) * inv_pos((u_p(1)-q1_loc(1,n))));
% % theta = atan((u_p(2)-q1(2)) * inv_pos(u_p(1)-q1(1)));
% Plos = 1/(1+e1*exp(-e2*(theta-e1)));
% a = sqrt(1/Nt) * (exp(imag * pi * cos (deg2rad(theta)) * (0:Nt-1)))'; %天线阵列响应
% kk = 0.2;
% beta0 = 1e-8;
% beta_d = beta0*(Plos+(1-Plos)*kk); %
% P1 = 10;
% noise_c = 1e-14;
% gamma_c_db= 20;
% % % gamma_c_db= 25;
% gamma_c = 10^(gamma_c_db/10);
% h = sqrt(beta_d* inv_pos(d1)) * a;
% gamma=P1*(norm(conj(h')*h))^2*inv_pos(noise_c);
% %
% e1 = 25;
% e2 = 0.112;
% theta = (180/pi) * acos(hm ./ sqrt(d1));
% %theta = atan((u_p(2)-q1_loc(2,n)) * inv_pos((u_p(1)-q1_loc(1,n))));
% Plos = 1/(1+e1*exp(-e2*(theta-e1)));
% a = sqrt(1/Nt) * (exp(-imag * pi * cos (deg2rad(theta)) * (0:Nt-1)))'; %天线阵列响应
% kk = 0.2;
% beta0 = 1e-8;
% beta_d = beta0*(Plos+(1-Plos)*kk); %
% % P1 = 10;
% noise_c = 1e-14;
% gamma_c_db= 20;
% % % gamma_c_db= 25;
% gamma_c = 10^(gamma_c_db/10);
% h = sqrt(beta_d* inv_pos(d1)) * a;
% gamma=Pa*(h'*h)^2*inv_pos(noise_c);
% pmax=2000; %w
pmax=1500;
p0=580.65; %w
p_ind=790.671; %w
U_tip=200; %m/s
v0=4.03; %m/s
d0=0.3;
tho=1.225; %kg/m^3
s=0.05; %m^3
A=0.79; %m^2
v1=norm([(q1(1)-q1_loc(1,n-1)),(q1(2)-q1_loc(2,n-1))])*inv_pos(deta_T);
v2=norm([(q2(1)-q2_loc(1,n-1)),(q2(2)-q2_loc(2,n-1))])*inv_pos(deta_T);
% v1=sqrt((q1(1)-q1_loc(1,n-1))^2+(q1(2)-q1_loc(2,n-1))^2)*inv_pos(deta_T);
% v2=sqrt((q2(1)-q2_loc(1,n-1))^2+(q2(2)-q2_loc(2,n-1))^2)*inv_pos(deta_T);
%x=pow_pos(v1,2)*inv_pos(2*v0^2);
% x=pow_pos((v1*inv_pos(sqrt(2)*v0)),2);
% s1_2=norm([1,x])-x;
v1_k=sqrt((q1_r(1)-q1_loc(1,n-1))^2+(q1_r(2)-q1_loc(2,n-1))^2)*inv_pos(deta_T);
v2_k=sqrt((q2_r(1)-q2_loc(1,n-1))^2+(q2_r(2)-q2_loc(2,n-1))^2)*inv_pos(deta_T);
% %对v1进行线性展开，转化为仿射表达式
% v11=v1_k^2*inv_pos(2*v0^2)+v1_k*(v1-v1_k)*inv_pos(v0^2);
% s1_2=norm([1,v11])-pow_pos(v1,2)*inv_pos(2*v0^2);
% %s1_2 = sqrt(1+v1^4/(4*v0^4))-v1^2/(2*v0^2);%s^2[n]
% s1=sqrt(s1_2);
s1=0.5*inv_pos((sqrt(1+v1_k^4/(4*v0^4))-v1_k^2/(2*v0^2))^0.5)*(2*v1_k^3*inv_pos((1+v1_k^4/(4*v0^4))^0.5)-v1_k/v0^2)*(v1-v1_k)+(sqrt(1+v1_k^4/(4*v0^4))-v1_k^2/(2*v0^2))^0.5;
P_1=p0*(1+3*pow_pos(v1,2)/(U_tip^2))+0.5*d0*tho*s*A*pow_pos(v1,2)*inv_pos(deta_T)+p_ind*s1;
% v22=v2_k^2*inv_pos(2*v0^2)+v2_k*(v2-v2_k)*inv_pos(v0^2);
% s2_2=norm([1,v22])-pow_pos(v2,2)*inv_pos(2*v0^2);
%s2_2 = sqrt(1+v2^4/(4*v0^4))-v2^2/(2*v0^2);%s^2[n]
% s2=sqrt(s2_2);
s2=0.5*inv_pos((sqrt(1+v2_k^4/(4*v0^4))-v2_k^2/(2*v0^2))^0.5)*(2*v2_k^3*inv_pos((1+v2_k^4/(4*v0^4))^0.5)-v2_k/v0^2)*(v2-v2_k)+(sqrt(1+v2_k^4/(4*v0^4))-v2_k^2/(2*v0^2))^0.5;
P_2=p0*(1+3*pow_pos(v2,2)/(U_tip^2))+0.5*d0*tho*s*A*pow_pos(v2,2)*inv_pos(deta_T)+p_ind*s2;
%
% v1_k=sqrt((q1_r(1)-q1_loc(1,n-1))^2+(q1_r(2)-q1_loc(2,n-1))^2)*inv_pos(deta_T);
% v2_k=sqrt((q2_r(1)-q2_loc(1,n-1))^2+(q2_r(2)-q2_loc(2,n-1))^2)*inv_pos(deta_T);
% s1_2_k = sqrt(1+v1_k^4/(4*v0^4))-v1_k^2/(2*v0^2);%s^2[n]
% s1_k=sqrt(s1_2_k);
%
% s2_2_k = sqrt(1+v2_k^4/(4*v0^4))-v2_k^2/(2*v0^2);%s^2[n]
% s2_k=sqrt(s2_2_k);
% g1=s1_2_k+2*s1_k*(s1-s1_k)+2*inv_pos((v0*deta_T)^2)*(q1_r(1)-q1_loc(1,n-1))*(q1(1)-q1_loc(1,n-1))+...
% 2*inv_pos((v0*deta_T)^2)*(q1_r(2)-q1_loc(2,n-1))*(q1(2)-q1_loc(2,n-1))-...
% 1*inv_pos((v0*deta_T)^2)*((q1_r(1)-q1_loc(1,n-1))^2)*((q1_r(2)-q1_loc(2,n-1))^2);
% g2=s2_2_k+2*s2_k*(s2-s2_k)+2*inv_pos((v0*deta_T)^2)*(q2_r(1)-q2_loc(1,n-1))*(q2(1)-q2_loc(1,n-1))+...
% 2*inv_pos((v0*deta_T)^2)*(q2_r(2)-q2_loc(2,n-1))*(q2(2)-q2_loc(2,n-1))-...
% 1*inv_pos((v0*deta_T)^2)*((q2_r(1)-q2_loc(1,n-1))^2)*((q2_r(2)-q2_loc(2,n-1))^2);
P_1<=pmax;
P_2<=pmax;
% 0<=inv_pos(s1_2)<=g1;
% 0<=inv_pos(s2_2)<=g2;
% Ih = [eye(Nt); h'];
% Ih * (-1-1/gamma_c) * Ih' +...
% [mu * eye(Nt), zeros(Nt,1); zeros(1,Nt), -mu * epsilon - noise_c] >= 0; %s-procedure过程的转化
% gamma>=gamma_c;
% Position constraints (44b)
norms(q1 - q1_loc(:,n-1)) <= v1max * deta_T;
norms(q2 - q2_loc(:,n-1)) <= v2max * deta_T;
% Safe distance constraints (44c)
% dismin^2 <= (2*(q1_r - q2_r)'*(q1 - q2))- (norm(q1_r - q2_r)^2);
dismin^2<=(q1_r - q2_r)'*(q1 - q2);
cvx_end
q1_r = q1; %优化后的无人机轨迹
q2_r = q2;
val(n-1,r) = cvx_optval; %存储的CVX的最优解
end
%% EKF measurement update part
% y = [norm(u_real(:,n)-q1)/c ; norm(u_real(:,n)-q2)/c];
% y = [(sqrt((norm(u_real(:,n)-q1)^2+hm^2)))/c ; (sqrt((norm(u_real(:,n)-q2)^2+hm^2)))/c];
% H = [(u_real(1,n)-q1(1))/(c*norm(u_real(:,n)-q1)), (u_real(2,n)-q1(2))/(c*norm(u_real(:,n)-q1)), 0, 0;
% (u_real(1,n)-q2(1))/(c*norm(u_real(:,n)-q2)), (u_real(2,n)-q2(2))/(c*norm(u_real(:,n)-q2)), 0, 0];
% d11 = norm(u_p-q1)^2;
% d22 = norm(u_p-q2)^2;
%雅可比矩阵
y = [(sqrt((norm(u_real(:,n)-q1)^2+hm^2)))/c ; (sqrt((norm(u_real(:,n)-q2)^2+hm^2)))/c];
H = [(u_real(1,n)-q1(1))/(c*(sqrt((norm(u_real(:,n)-q1)^2+hm^2)))), (u_real(2,n)-q1(2))/(c*(sqrt((norm(u_real(:,n)-q1)^2+hm^2)))), 0, 0;
(u_real(1,n)-q2(1))/(c*(sqrt((norm(u_real(:,n)-q2)^2+hm^2)))), (u_real(2,n)-q2(2))/(c*(sqrt((norm(u_real(:,n)-q2)^2+hm^2)))), 0, 0];
d11 = norm(u_real(:,n)-q1)^2+hm^2;
d22 = norm(u_real(:,n)-q2)^2+hm^2;
Sigma_tau11 = a2^2*Sigma*d11^2*inv_pos(Pa*Nsym*Nt*Nr*Beta_r);
Sigma_tau22 = a2^2*Sigma*d22^2*inv_pos(Pa*Nsym*Nt*Nr*Beta_r);
R = [Sigma_tau11,0;
0, Sigma_tau22];
K = M_p * conj(H') * pinv(R + H * M_p * conj(H')); %卡尔曼增益
% y_p = [norm(q1-u_p)/c; norm(q2-u_p)/c];
y_p = [(sqrt((norm(u_p-q1)^2+hm^2)))/c; (sqrt((norm(u_p-q2)^2+hm^2)))/c];
x = x_p + K * (y - y_p);
M = (eye(4) - K * H) * M_p;
q1_loc(:,n) = q1;
q2_loc(:,n) = q2;
% val(n) = cvx_optval; %存储的CVX的最优解
x_estimated(:,n) = x;
disp(['Time slot: ', num2str(n)]);
% y = [(sqrt((norm(u_real(:,n)-q1_loc(:,n))^2+hm^2)))/c ; (sqrt((norm(u_real(:,n)-q2_loc(:,n))^2+hm^2)))/c];
% H = [(u_real(1,n)-q1_loc(1,n))/(c*(sqrt((norm(u_real(:,n)-q1_loc(:,n))^2+hm^2)))), (u_real(2,n)-q1_loc(2,n))/(c*(sqrt((norm(u_real(:,n)-q1_loc(:,n))^2+hm^2)))), 0, 0;
% (u_real(1,n)-q2_loc(1,n))/(c*(sqrt((norm(u_real(:,n)-q2_loc(:,n))^2+hm^2)))), (u_real(2,n)-q2_loc(2,n))/(c*(sqrt((norm(u_real(:,n)-q2_loc(:,n))^2+hm^2)))), 0, 0];
% d11 = norm(u_real(:,n)-q1_loc(:,n))^2+hm^2;
% d22 = norm(u_real(:,n)-q2_loc(:,n))^2+hm^2;
% Sigma_tau11 = a2^2*Sigma*d11^2*inv_pos(Pa*Nsym*Nt*Nr*Beta_r);
% Sigma_tau22 = a2^2*Sigma*d22^2*inv_pos(Pa*Nsym*Nt*Nr*Beta_r);
% R = [Sigma_tau11,0;
% 0, Sigma_tau22];
% K = M_p * conj(H') * pinv(R + H * M_p * conj(H')); %卡尔曼增益
% % y_p = [norm(q1-u_p)/c; norm(q2-u_p)/c];
% y_p = [(sqrt((norm(u_p-q1_loc(:,n))^2+hm^2)))/c; (sqrt((norm(u_p-q2_loc(:,n))^2+hm^2)))/c];
% x = x_p + K * (y - y_p);
% M = (eye(4) - K * H) * M_p;
% q1_loc(:,n+1) = q1;
% q2_loc(:,n+1) = q2;
% val(n) = cvx_optval; %存储的CVX的最优解
% x_estimated(:,n+1) = x;
% disp(['Time slot: ', num2str(n)]);
end
save(‘uav1.mat’,‘q1_loc’);
save(‘uav2.mat’,‘q2_loc’);
%% Plot trajectory
figure;
hold on;
grid on;
plot(q1_loc(1,1:n-1), q1_loc(2,1:n-1), ‘b-d’, ‘linewidth’, 1.5);
plot(q2_loc(1,1:n-1), q2_loc(2,1:n-1), ‘k-o’, ‘linewidth’, 1.5);
plot(u_real(1,1:n-1), u_real(2,1:n-1), ‘r-v’, ‘LineWidth’, 1.5);
plot(x_estimated(1,1:n-1), x_estimated(2,1:n-1), ‘y-s’, ‘LineWidth’, 1.5);
% % 获取当前坐标轴的句柄
% ax = gca;
% % 设置横坐标的步长为10
% set(ax, ‘xtick’, 100:10:250);
% % 设置纵坐标的步长为10
% set(ax, ‘ytick’, 100:10:250);
legend(‘UAV1 trajectory’, ‘UAV2 trajectory’, ‘MT trajectory’,‘EKF tracking’,‘Location’, ‘southeast’);
xlabel(‘x (m)’);
ylabel(‘y (m)’);
box on;
What proof do you have that your iterative method for the EKF converges to anything, let alone a global or even local minimum? You also seem to have bad numerical scaling, with some input data being very small or large magnitude; and that’s on the first iteration. Subsequent iterations could get more and more extreme.
EKF’s can be tricky. Your crude implementation might not be up to the job.
I am currently trying to use Successive Convex Approximation (SCA) to transform the non-convex objective function into a convex one, and then use CVX to solve the minimization problem. The Extended Kalman Filter (EKF) is only used for tracking afterwards and is not involved in the CVX solving process. However, during the SCA iteration, the optimal solution keeps increasing as the number of iterations increases. What should I do now
Everything I wrote about EKF also applies to SCA.
Getting SCA to run nicely is out of scope of this forum. SCA is unreliable, and many of the algorithms are too complicated for volunteers with limited time to be able to spend adequate effort to figure out things.
An alternative is to use a non-convex nonlinear solver; for instance, under YALMIP.
Okay, but I’m not very familiar with YALMIP. Are there any other solutions if I still choose to use CVX for solving
You can submit any sequence of convex optimization problems which follow CVX’s rules. how you come up with these is up to you.