I am new to control systems and have tried to use GEKKO. For my model simulation geos correct and run however at MHE and also at MPC It runs correctly when t = 15 seconds after that if t = 16 seconds at the 12th it goes skeptical as shown on the attached figure. Is there a reason for that?
m = GEKKO(remote = False)
g = m.Const(value = 9.81)
Cc = m.Const(value = 2*10**-5)
D1 = m.Const(value = 0.1016)
D2 = m.Const(value = 0.1016)
h1 = m.Const(value = 100)
hv = m.Const(value = 1000)
L1 = m.Const(value = 500)
L2 = m.Const(value = 1100)
V1 = m.Const(value = 4.054)
V2 = m.Const(value = 9.729)
A1 = m.Const(0.008107)
A2 = m.Const(0.008107)
beta1 = m.Const(value = 1.5 * 10**9)
beta2 = m.Const(value = 1.5* 10**9)
Pres = m.Const(value = 1.261 * 10**7)
M = m.Const(value = 1.992 * 10**8)
rho = m.Const(value = 932)
PI = m.Const(value = 2.32 * 10**(-9))
visc = m.Const(value = 0.024)
fo = m.Const(value = 60)
Inp = m.Const(value = 65)
Pnp = m.Const(value = 1.625 * 10**5)
z = m.MV(1, lb = 0.1, ub = 1)
f = m.MV(35, lb = 35, ub = 65)
f.STATUS = 1
f.DMAX = 30
f.DCOST = 0.0002 # slow down change of frequency
f.UPPER = 65
Ppin = m.CV()
Ppin.STATUS = 1 # add the SP to the objective
Ppin.SP = 6e6 # set point
Ppin.TR_INIT = 1 # set point trajectory
Ppin.TAU = 5 # time constant of trajectory
Pwf = m.Var(ub = Pres)
Ppout = m.Var()
Pwh = m.Var()
Pm = m.Var()
P_fric_drop = m.Var()
F1 = m.Var()
F2 = m.Var()
q = m.Var(lb = 0)
qc = m.Var(lb = 0)
qr = m.Var(lb = 0)
I = m.Var()
H = m.Var()
BHP = m.Var(lb = 0)
BHPo = m.Var()
qo = m.Var()
Ho = m.Var()
m.Equation([Pwh.dt() == beta2*(q-qc)/V2,
Pwf.dt() == beta1*(qr-q)/V1,
q.dt() == (1/M)*(Pwf - Pwh - rho*g*hv - P_fric_drop + (Ppout-Ppin))])
m.Equation([qr == PI*(Pres - Pwf),
qc == Cc*z*(m.sqrt((Pwh - Pm))),
Pm == Pwh/2,
P_fric_drop == F1 + F2,
F1 == 0.158 * rho * L1* q**2 * visc**0.5 /(D1 * A1**2 * (rho*D1*q)**0.5),
F2 == 0.158 * rho * L2* q**2 * visc**0.5 /(D2 * A2**2 * (rho*D2*q)**0.5)])
m.Equation([
qo == q * fo/f,
H == Ho * (f/fo)**2,
BHPo == -2.3599e9*qo**3 - 1.8082e7*qo**2 + 4.3346e6*qo + 9.4355e4,
BHP == BHPo * (f/fo)**3,
Ppin == Pwf - rho*g*h1 - F1,
Ho == 9.5970e2+7.4959e3*qo+-1.2454e6*qo**2,
I == Inp * BHP / Pnp,
Ppout == H*rho*g + Ppin
])
m.options.solver = 1
m.options.MAX_ITER = 250
m.options.IMODE = 1
m.solve(debug=True)
tf = 30 # final time (sec)
st = 2.0 # simulation time intervals
nt = int(tf/st)+1 # simulation time points
m.time = np.linspace(0,tf,nt)
m.options.CV_TYPE = 2 # squared error
m.options.solver = 3
m.options.NODES=2
m.options.IMODE = 5
m.solve(debug = False)
I wasn't able to reproduce the problem you encountered so I'll give a few specific suggestions for troubleshooting your application and improving the ability of the solver to find a solution.
Help the Solver Find a Solution
Avoid sqrt(-negative)
#qc == Cc*z*(m.sqrt((Pwh - Pm)))
qc**2 == Cc**2 * z**2 * (Pwh - Pm) # avoid negative in sqrt
Avoid potential for divide by zero. The value of q is constrained away from zero but look for other equations that could be improved by re-arrangement.
#qo == q * fo/f
qo * f == q * fo
#H == Ho * (f/fo)**2
H * fo**2 == Ho * f**2
#I == Inp * BHP / Pnp
I * Pnp == Inp * BHP
Constrain the FVs and MVs
It can often help to put small DMAX or limit the upper and lower bound for FVs and MVs. Constraints on the Var types can lead to infeasible solutions.
Reduce the degrees of freedom
For MHE applications, I recommend that you use FVs instead of MVs for your unknown parameters. The FVs calculate a single value over all of the measurements. The MVs calculate a new parameter value for every time point. Using an FV instead of an MV can help the solver because there are fewer decisions for the optimizer and the parameter values won't be adjusted to track signal noise.
Check the Solutions
Configure the objective function.
For MHE applications, you need to have some CV values that you are trying to match that have FSTATUS=1. In the example you posted there are no CVs with FSTATUS=1 or any inserted measurements. MHE is to align a model with measured CVs values by adjusting the unknown parameters as FVs or MVs.
Check the solver status to make sure that it found a successful solution with APPSTAUTS==1
if m.options.APPSTATUS==1:
print('Successful Solution)
else:
print('Solver Failed to Find a Solution')
Create plots of the variables, especially if there is a problematic period. You can often see where there is an integrating variable such as qr or qc that are approaching a variable constraint. Here is an example where I converted your MHE application into an MPC application. I'm adjusting the friction factor f (not realistic) and the valve opening z to show how the MPC drives Ppin to the target set point.
from gekko import GEKKO
import numpy as np
import matplotlib.pyplot as plt
m = GEKKO(remote = False)
g = m.Const(value = 9.81)
Cc = m.Const(value = 2*10**-5)
D1 = m.Const(value = 0.1016)
D2 = m.Const(value = 0.1016)
h1 = m.Const(value = 100)
hv = m.Const(value = 1000)
L1 = m.Const(value = 500)
L2 = m.Const(value = 1100)
V1 = m.Const(value = 4.054)
V2 = m.Const(value = 9.729)
A1 = m.Const(0.008107)
A2 = m.Const(0.008107)
beta1 = m.Const(value = 1.5 * 10**9)
beta2 = m.Const(value = 1.5* 10**9)
Pres = m.Const(value = 1.261 * 10**7)
M = m.Const(value = 1.992 * 10**8)
rho = m.Const(value = 932)
PI = m.Const(value = 2.32 * 10**(-9))
visc = m.Const(value = 0.024)
fo = m.Const(value = 60)
Inp = m.Const(value = 65)
Pnp = m.Const(value = 1.625 * 10**5)
z = m.MV(1, lb = 0.1, ub = 1)
z.STATUS = 1
z.DMAX = 0.01
f = m.MV(35, lb = 35, ub = 65)
f.STATUS = 1
f.DMAX = 30
f.DCOST = 0.0002 # slow down change of frequency
f.UPPER = 65
Ppin = m.CV()
Ppin.STATUS = 1 # add the SP to the objective
Ppin.SP = 6e6 # set point
Ppin.TR_INIT = 1 # set point trajectory
Ppin.TAU = 5 # time constant of trajectory
Pwf = m.Var(ub = Pres)
Ppout = m.Var()
Pwh = m.Var()
Pm = m.Var()
P_fric_drop = m.Var()
F1 = m.Var()
F2 = m.Var()
q = m.Var(lb = 0)
qc = m.Var(lb = 0)
qr = m.Var(lb = 0)
I = m.Var()
H = m.Var()
BHP = m.Var(lb = 0)
BHPo = m.Var()
qo = m.Var()
Ho = m.Var()
m.Equation([Pwh.dt() == beta2*(q-qc)/V2,
Pwf.dt() == beta1*(qr-q)/V1,
M * q.dt() == Pwf - Pwh - rho*g*hv - P_fric_drop + (Ppout-Ppin)])
m.Equation([qr == PI*(Pres - Pwf),
qc == Cc*z*(m.sqrt((Pwh - Pm))),
Pm == Pwh/2,
P_fric_drop == F1 + F2,
F1 == 0.158 * rho * L1* q**2 * visc**0.5 /(D1 * A1**2 * (rho*D1*q)**0.5),
F2 == 0.158 * rho * L2* q**2 * visc**0.5 /(D2 * A2**2 * (rho*D2*q)**0.5)])
m.Equation([
qo * f == q * fo,
H * fo**2 == Ho * f**2,
BHPo == -2.3599e9*qo**3 - 1.8082e7*qo**2 + 4.3346e6*qo + 9.4355e4,
BHP == BHPo * (f/fo)**3,
Ppin == Pwf - rho*g*h1 - F1,
Ho == 9.5970e2+7.4959e3*qo+-1.2454e6*qo**2,
I * Pnp == Inp * BHP,
Ppout == H*rho*g + Ppin
])
m.options.solver = 1
m.options.MAX_ITER = 250
m.options.IMODE = 1
m.solve()
tf = 30 # final time (sec)
st = 2.0 # simulation time intervals
nt = int(tf/st)+1 # simulation time points
m.time = np.linspace(0,tf,nt)
m.options.CV_TYPE = 2 # squared error
m.options.solver = 3
m.options.NODES=2
m.options.IMODE = 6
plt.figure(figsize=(10,5))
for i in range(10):
m.solve(disp=False)
print(i,m.options.APPSTATUS)
plt.subplot(10,1,i+1)
plt.plot(m.time+i*st,Ppin.value)
plt.xlim([0,50])
plt.ylim([6e6,7e6])
plt.show()
Your application looks like it is for hydraulic pressure control in drilling. There are additional models available on GitHub. I hope you'll consider contributing your model or case studies to the open-source repository.
I need help with a dynamic optimization problem that consist in a consumed energy optimization of a UAV with this optimal control problem.
enter image description here
enter image description here
My code is this
Ecuations:
Parameters
tf
#Velocidad de rotores rad/s
#Las condiciones iniciales permiten igualar la acción de la gravedad
#Se tomo 4000rad/s como la velocidad maxima de los rotores
w1 = 912.32, >=0, <=3000
w2 = 912.32, >=0, <=3000
w3 = 912.32, >=0, <=3000
w4 = 912.32, >=0, <=3000
t1 = 0, >=0
t2 = 0, >=0
t3 = 0, >=0
t4 = 0, >=0
Constants
!----------------COEFICIENTES DEL MODELO-----------------!
#Gravedad
g = 9.81 !m/s^2
pi = 3.14159265359
#Motor Coefficients
J = 4.1904e-5 !kg*m^2
kt = 0.0104e-3 !N*m/A
kv = 96.342 !rad/s/volt
Dv = 0.2e-3 !N*m*s/rad
R = 0.2 !Ohms
#Battery parameters
Q = 1.55 !Ah
Rint = 0.02 !Ohms
E0 = 1.24 !volt
K = 2.92e-3 !volt
A = 0.156
B =2.35
#Quadrotor parameters
l = 0.175 !m
m = 1.3 !kg
Ix = 0.081 !kg*m^2
Iy = 0.081 !kg*m^2
Iz = 0.142 !kg*m^2
kb = 3.8305e-6 !N/rad/s
ktau = 2.2518e-8 !(N*m)/rad/s
#Parametrizacion del polinomio
a1 = -1.72e-5
a2 = 1.95e-5
a3 = -6.98e-6
a4 = 4.09e-7
b1 = 0.014
b2 = -0.0157
b3 = 5.656e-3
b4 = -3.908e-4
c1 = -0.8796
c2 = 0.3385
c3 = 0.2890
c4 = 0.1626
Variables
!------------------CONDICONES INICIALES------------------!
x = 0
xp = 0
y = 0
yp = 0
z = 0
zp = 0
pitch = 0, >=-pi/2, <=pi/2 !theta - restricciones
pitchp = 0
roll = 0, >=-pi/2, <=pi/2 !phi - restricciones
rollp = 0
yaw = 0 !psi
yawp = 0%, >=-200/180, <=200/180
#Función objetivo
of = 0 !condición inicial de la función objetivo
Intermediates
#Motor 1
aw1 = a1*w1^2 + b1*w1 + c1
bw1 = a2*w1^2 + b2*w1 + c2
cw1 = a3*w1^2 + b3*w1 + c3
dw1 = a4*w1^2 + b4*w1 + c4
#Motor 2
aw2 = a1*w2^2 + b1*w2 + c1
bw2 = a2*w2^2 + b2*w2 + c2
cw2 = a3*w2^2 + b3*w2 + c3
dw2 = a4*w2^2 + b4*w2 + c4
#Motor 3
aw3 = a1*w3^2 + b1*w3 + c1
bw3 = a2*w3^2 + b2*w3 + c2
cw3 = a3*w3^2 + b3*w3 + c3
dw3 = a4*w3^2 + b4*w3 + c4
#Motor 4
aw4 = a1*w4^2 + b1*w4 + c1
bw4 = a2*w4^2 + b2*w4 + c2
cw4 = a3*w4^2 + b3*w4 + c3
dw4 = a4*w4^2 + b4*w4 + c4
#frj(wj(t),Tj(t))
fr1=aw1*t1^3 + bw1*t1^2 + cw1*t1 + dw1
fr2=aw2*t2^3 + bw2*t2^2 + cw2*t2 + dw2
fr3=aw3*t3^3 + bw3*t3^2 + cw3*t3 + dw3
fr4=aw4*t4^3 + bw4*t4^2 + cw4*t4 + dw4
!---------------------CONTROL INPUTS---------------------!
T = kb * (w1^2 + w2^2 + w3^2 + w4^2)
u1 = kb * (w2^2 - w4^2)
u2 = kb * (w3^2 - w1^2)
u3 = ktau * (w1^2 - w2^2 + w3^2 - w4^2)
wline = w1 - w2 + w3 - w4
!-------------------ENERGIA POR ROTOR--------------------!
Ec1 = ((J*$w1 + ktau*w1^2 + Dv*w1)/fr1)*w1
Ec2 = ((J*$w2 + ktau*w2^2 + Dv*w2)/fr2)*w2
Ec3 = ((J*$w3 + ktau*w3^2 + Dv*w3)/fr3)*w3
Ec4 = ((J*$w4 + ktau*w4^2 + Dv*w4)/fr4)*w4
Ectotal = Ec1 + Ec2 + Ec3 + Ec4
Equations
!---------------MINIMIZAR FUNCIÓN OBJETIVO---------------!
minimize tf * of
!-----------------RELACION DE VARIABLES------------------!
xp = $x
yp = $y
zp = $z
pitchp = $pitch
rollp = $roll
yawp = $yaw
!-----------------CONDICONES DE FRONTERA-----------------!
#Condiciones finales del modelo
tf * x = 4
tf * y = 5
tf * z = 6
tf * xp = 0
tf * yp = 0
tf * zp = 0
tf * roll = 0
tf * pitch = 0
tf * yaw = 0
!-----------------TORQUE DE LOS MOTORES------------------!
t1 = J*$w1 + ktau*w1^2 + Dv*w1
t2 = J*$w2 + ktau*w2^2 + Dv*w2
t3 = J*$w3 + ktau*w3^2 + Dv*w3
t4 = J*$w4 + ktau*w4^2 + Dv*w4
!------------------------SUJETO A------------------------!
#Modelo aerodinámico del UAV
m*$xp = (cos(roll)*sin(pitch)*cos(yaw) + sin(roll)*sin(yaw))*T
m*$yp = (cos(roll)*sin(pitch)*sin(yaw) - sin(roll)*cos(yaw))*T
m*$zp = (cos(roll)*cos(pitch))*T-m*g
Ix*$rollp = ((Iy - Iz)*pitchp*yawp + J*pitchp*wline + l*u1)
Iy*$pitchp = ((Iz - Ix)*rollp*yawp - J*rollp*wline + l*u2)
Iz*$yawp = ((Ix - Iy)*rollp*pitchp + u3)
!--------------------FUNCIÓN OBJETIVO--------------------!
$of = Ectotal
MATLAB:
clear all; close all; clc
server = 'http://127.0.0.1';
app = 'traj_optima';
addpath('C:/Program Files/MATLAB/apm_matlab_v0.7.2/apm')
apm(server,app,'clear all');
apm_load(server,app,'ecuaciones_mod.apm');
csv_load(server,app,'tiempo2.csv');
apm_option(server,app,'apm.max_iter',200);
apm_option(server,app,'nlc.nodes',3);
apm_option(server,app,'apm.rtol',1);
apm_option(server,app,'apm.otol',1);
apm_option(server,app,'nlc.solver',3);
apm_option(server,app,'nlc.imode',6);
apm_option(server,app,'nlc.mv_type',1);
costo=1e-5;%1e-5
%VARIABLES CONTROLADAS
%Velocidades angulares
apm_info(server,app,'MV','w1');
apm_option(server,app,'w1.status',1);
apm_info(server,app,'MV','w2');
apm_option(server,app,'w2.status',1);
apm_info(server,app,'MV','w3');
apm_option(server,app,'w3.status',1);
apm_info(server,app,'MV','w4');
apm_option(server,app,'w4.status',1);
% Torques
apm_info(server,app,'MV','t1');
apm_option(server,app,'t1.status',1);
apm_info(server,app,'MV','t2');
apm_option(server,app,'t2.status',1);
apm_info(server,app,'MV','t3');
apm_option(server,app,'t3.status',1);
apm_info(server,app,'MV','t4');
apm_option(server,app,'t4.status',1);
%Salida
output = apm(server,app,'solve');
disp(output)
y = apm_sol(server,app);
z = y.x;
tiempo2.csv
time,tf
0,0
0.001,0
0.2,0
0.4,0
0.6,0
0.8,0
1,0
1.2,0
1.4,0
1.6,0
1.8,0
2,0
2.2,0
2.4,0
2.6,0
2.8,0
3,0
3.2,0
3.4,0
3.6,0
3.8,0
4,0
4.2,0
4.4,0
4.6,0
4.8,0
5,0
5.2,0
5.4,0
5.6,0
5.8,0
6,0
6.2,0
6.4,0
6.6,0
6.8,0
7,0
7.2,0
7.4,0
7.6,0
7.8,0
8,0
8.2,0
8.4,0
8.6,0
8.8,0
9,0
9.2,0
9.4,0
9.6,0
9.8,0
10,1
Finally the answer obtained is:
enter image description here
I need help with this local infeasibility problem, please.
The infeasible solution is caused by the terminal constraints:
tf * z = 4
tf * z = 5
tf * z = 6
When tf=0, the constraints are evaluated to 0=4, 0=5, 0=6 and the solver reports that these can not be satisfied by the solver. Instead, you can pose the constraints as:
tf * (x-4) = 0
tf * (y-5) = 0
tf * (z-6) = 0
That way, the constraint is valid when tf=0 and when tf=1 at the final time. A potential better way yet is to convert the terminal constraints to objective terms with f=1000 such as:
minimize f*tf*((x-4)^2 + (y-5)^2 + (z-6)^2)
minimize f*tf*(xp^2 + yp^2 + zp^2)
minimize f*tf*(roll^2 + pitch^2 + yaw^2)
That way, the optimizer won't report an infeasible solution if it can't reach the terminal constraints as discussed in the pendulum problem. I made a few other modifications to your model and script to achieve a successful solution. Here is a summary:
Converted terminal constraints to objective function (soft constraints)
Parameters t1-t4 should be variables
Fixed degree of freedom issue by making w1-w4 variables and w1p-w4p variables. w1-w4 are differential states.
Added constraints to w1p-w4p between -10 and 10 to help the solver converge
Added initialization step to simulate the model before optimizing. There are more details on initialization strategies in this paper: Safdarnejad, S.M., Hedengren, J.D., Lewis, N.R., Haseltine, E., Initialization Strategies for Optimization of Dynamic Systems, Computers and Chemical Engineering, 2015, Vol. 78, pp. 39-50, DOI: 10.1016/j.compchemeng.2015.04.016
Model
Parameters
tf
w1p = 0 > -10 < 10
w2p = 0 > -10 < 10
w3p = 0 > -10 < 10
w4p = 0 > -10 < 10
Constants
!----------------COEFICIENTES DEL MODELO-----------------!
#Gravedad
g = 9.81 !m/s^2
pi = 3.14159265359
#Motor Coefficients
J = 4.1904e-5 !kg*m^2
kt = 0.0104e-3 !N*m/A
kv = 96.342 !rad/s/volt
Dv = 0.2e-3 !N*m*s/rad
R = 0.2 !Ohms
#Battery parameters
Q = 1.55 !Ah
Rint = 0.02 !Ohms
E0 = 1.24 !volt
K = 2.92e-3 !volt
A = 0.156
B =2.35
#Quadrotor parameters
l = 0.175 !m
m = 1.3 !kg
Ix = 0.081 !kg*m^2
Iy = 0.081 !kg*m^2
Iz = 0.142 !kg*m^2
kb = 3.8305e-6 !N/rad/s
ktau = 2.2518e-8 !(N*m)/rad/s
#Parametrizacion del polinomio
a1 = -1.72e-5
a2 = 1.95e-5
a3 = -6.98e-6
a4 = 4.09e-7
b1 = 0.014
b2 = -0.0157
b3 = 5.656e-3
b4 = -3.908e-4
c1 = -0.8796
c2 = 0.3385
c3 = 0.2890
c4 = 0.1626
Variables
!------------------CONDICONES INICIALES------------------!
x = 0
xp = 0
y = 0
yp = 0
z = 0
zp = 0
pitch = 0, >=-pi/2, <=pi/2 !theta - restricciones
pitchp = 0
roll = 0, >=-pi/2, <=pi/2 !phi - restricciones
rollp = 0
yaw = 0 !psi
yawp = 0 %, >=-200/180, <=200/180
#Velocidad de rotores rad/s
#Las condiciones iniciales permiten igualar la acción de la gravedad
#Se tomo 4000rad/s como la velocidad maxima de los rotores
w1 = 912.32, >=0, <=3000
w2 = 912.32, >=0, <=3000
w3 = 912.32, >=0, <=3000
w4 = 912.32, >=0, <=3000
t1 = 0, >=0
t2 = 0, >=0
t3 = 0, >=0
t4 = 0, >=0
#Función objetivo
of = 0 !condición inicial de la función objetivo
Intermediates
#Motor 1
aw1 = a1*w1^2 + b1*w1 + c1
bw1 = a2*w1^2 + b2*w1 + c2
cw1 = a3*w1^2 + b3*w1 + c3
dw1 = a4*w1^2 + b4*w1 + c4
#Motor 2
aw2 = a1*w2^2 + b1*w2 + c1
bw2 = a2*w2^2 + b2*w2 + c2
cw2 = a3*w2^2 + b3*w2 + c3
dw2 = a4*w2^2 + b4*w2 + c4
#Motor 3
aw3 = a1*w3^2 + b1*w3 + c1
bw3 = a2*w3^2 + b2*w3 + c2
cw3 = a3*w3^2 + b3*w3 + c3
dw3 = a4*w3^2 + b4*w3 + c4
#Motor 4
aw4 = a1*w4^2 + b1*w4 + c1
bw4 = a2*w4^2 + b2*w4 + c2
cw4 = a3*w4^2 + b3*w4 + c3
dw4 = a4*w4^2 + b4*w4 + c4
#frj(wj(t),Tj(t))
fr1=aw1*t1^3 + bw1*t1^2 + cw1*t1 + dw1
fr2=aw2*t2^3 + bw2*t2^2 + cw2*t2 + dw2
fr3=aw3*t3^3 + bw3*t3^2 + cw3*t3 + dw3
fr4=aw4*t4^3 + bw4*t4^2 + cw4*t4 + dw4
!---------------------CONTROL INPUTS---------------------!
T = kb * (w1^2 + w2^2 + w3^2 + w4^2)
u1 = kb * (w2^2 - w4^2)
u2 = kb * (w3^2 - w1^2)
u3 = ktau * (w1^2 - w2^2 + w3^2 - w4^2)
wline = w1 - w2 + w3 - w4
!-------------------ENERGIA POR ROTOR--------------------!
Ec1 = ((J*$w1 + ktau*w1^2 + Dv*w1)/fr1)*w1
Ec2 = ((J*$w2 + ktau*w2^2 + Dv*w2)/fr2)*w2
Ec3 = ((J*$w3 + ktau*w3^2 + Dv*w3)/fr3)*w3
Ec4 = ((J*$w4 + ktau*w4^2 + Dv*w4)/fr4)*w4
Ectotal = Ec1 + Ec2 + Ec3 + Ec4
! scaling factor for terminal constraint
f = 1000
Equations
!---------------MINIMIZAR FUNCIÓN OBJETIVO---------------!
minimize tf * of
!-----------------RELACION DE VARIABLES------------------!
xp = $x
yp = $y
zp = $z
pitchp = $pitch
rollp = $roll
yawp = $yaw
w1p = $w1
w2p = $w2
w3p = $w3
w4p = $w4
!-----------------CONDICONES DE FRONTERA-----------------!
#Condiciones finales del modelo
#tf * (x-4) = 0
#tf * (y-5) = 0
#tf * (z-6) = 0
#tf * xp = 0
#tf * yp = 0
#tf * zp = 0
#tf * roll = 0
#tf * pitch = 0
#tf * yaw = 0
minimize f*tf*((x-4)^2 + (y-5)^2 + (z-6)^2)
minimize f*tf*(xp^2 + yp^2 + zp^2)
minimize f*tf*(roll^2 + pitch^2 + yaw^2)
!-----------------TORQUE DE LOS MOTORES------------------!
t1 = J*w1p + ktau*w1^2 + Dv*w1
t2 = J*w2p + ktau*w2^2 + Dv*w2
t3 = J*w3p + ktau*w3^2 + Dv*w3
t4 = J*w4p + ktau*w4^2 + Dv*w4
!------------------------SUJETO A------------------------!
#Modelo aerodinámico del UAV
m*$xp = (cos(roll)*sin(pitch)*cos(yaw) + sin(roll)*sin(yaw))*T
m*$yp = (cos(roll)*sin(pitch)*sin(yaw) - sin(roll)*cos(yaw))*T
m*$zp = (cos(roll)*cos(pitch))*T-m*g
Ix*$rollp = ((Iy - Iz)*pitchp*yawp + J*pitchp*wline + l*u1)
Iy*$pitchp = ((Iz - Ix)*rollp*yawp - J*rollp*wline + l*u2)
Iz*$yawp = ((Ix - Iy)*rollp*pitchp + u3)
!--------------------FUNCIÓN OBJETIVO--------------------!
$of = Ectotal
MATLAB Script
clear all; close all; clc
server = 'http://byu.apmonitor.com';
app = 'traj_optima';
addpath('apm')
apm(server,app,'clear all');
apm_load(server,app,'ecuaciones_mod.apm');
csv_load(server,app,'tiempo2.csv');
apm_option(server,app,'apm.max_iter',1000);
apm_option(server,app,'apm.nodes',3);
apm_option(server,app,'apm.rtol',1e-6);
apm_option(server,app,'apm.otol',1e-6);
apm_option(server,app,'apm.solver',3);
apm_option(server,app,'apm.imode',6);
apm_option(server,app,'apm.mv_type',1);
costo=1e-5;%1e-5
%VARIABLES CONTROLADAS
%Velocidades angulares
apm_info(server,app,'MV','w1p');
apm_option(server,app,'w1p.status',1);
apm_info(server,app,'MV','w2p');
apm_option(server,app,'w2p.status',1);
apm_info(server,app,'MV','w3p');
apm_option(server,app,'w3p.status',1);
apm_info(server,app,'MV','w4p');
apm_option(server,app,'w4p.status',1);
%Salida
disp('')
disp('------------- Initialize ----------------')
apm_option(server,app,'apm.coldstart',1);
output = apm(server,app,'solve');
disp(output)
disp('')
disp('-------------- Optimize -----------------')
apm_option(server,app,'apm.time_shift',0);
apm_option(server,app,'apm.coldstart',0);
output = apm(server,app,'solve');
disp(output)
y = apm_sol(server,app);
z = y.x;
This gives a successful solution but the terminal constraints are not met. The solver optimizes the use of w1p-w4p to minimize the objective but there is no solution that makes it to the terminal constraints.
The solution was found.
The final value of the objective function is 50477.4537378181
---------------------------------------------------
Solver : IPOPT (v3.12)
Solution time : 3.06940000000759 sec
Objective : 50477.4537378181
Successful solution
---------------------------------------------------
As a next step, I recommend that you increase the number of time points or allow the final time to change to meet the terminal constraints. You may also want to consider switching to Python Gekko that uses the same underlying engine as APM MATLAB. In this case, the modeling language is fully integrated with Python.
I have this equation and want to solve it for v. I tried Mathematica but it is not able to do it. Is there any software, language capable of solving it?
Equation:
Solve[1 + 0.0914642/v^5 - 1.87873/v^4 + 96.1878/v^2 - (
17.3914 E^(-(0.0296/v^2)) (1.398 + 0.0296/v^2))/v^2 - 0.947895/v -
1.37421 v == 0, v]
The text file/m-file is here.
Using Mathematica 9 :-
Clear[v]
expr = 1 + 0.0914642/v^5 - 1.87873/v^4 + 96.1878/v^2 - (
17.3914 E^(-(0.0296/v^2)) (1.398 + 0.0296/v^2))/v^2 - 0.947895/v -
1.37421 v;
sol = Solve[expr == 0, v, Reals]
{{v -> -0.172455}, {v -> 0.0594091}, {v -> 0.105179}, {v -> 3.93132}}
Checking solutions :-
roots = v /. sol;
(v = #; expr) & /# roots
{2.27374*10^-13, 2.32703*10^-12, -9.66338*10^-13, -1.77636*10^-15}
(v = #; Chop[expr]) & /# roots
{0, 0, 0, 0}
Try this in Matlab. You need to have the Symbolic Math Toolbox installed:
>> syms v %// declare symbolic variable, used in defining y
>> y = 1 + 0.0914642/v^5 - 1.87873/v^4 + 96.1878/v^2 - (17.3914*exp(-(0.0296/v^2)) * (1.398 + 0.0296/v^2))/v^2 - 0.947895/v - 1.37421*v;
>> solve(y,v) %// seeks zeros of y as a function of v
ans =
3.931322452560060553464772086259
>> subs(y,3.931322452560060553464772086259) %// check
ans =
-4.4409e-016 %// almost 0 (precision of floating point numbers): it is correct
Without the symbolic math toolbox, you can still do it numerically with fzero:
a1 = 8.99288497*10^(-2);
a2 = -4.94783127*10^(-1);
a3 = 4.77922245*10^(-2);
a4 = 1.03808883*10^(-2);
a5 = -2.82516861*10^(-2);
a6 = 9.49887563*10^(-2);
a7 = 5.20600880*10^(-4);
a8 = -2.93540971*10^(-4);
a9 = -1.77265112*10^(-3);
a10 = -2.51101973*10^(-5);
a11 = 8.93353441*10^(-5);
a12 = 7.88998563*10^(-5);
a13 = -1.66727022*10^(-2);
a14 = 1.39800000 * exp(0);
a15 = 2.96000000*10^(-2);
t = 30;
p = 10;
tr = t/(273.15 + 31.1);
pr = p/(73.8);
s1 = #(v) (a1 + (a2/tr^2) + (a3/tr^3))./v;
s2 = #(v) (a4 + (a5/tr^2) + (a6/tr^3))./v.^2;
s3 = #(v) (a7 + (a8/tr^2) + (a9/tr^3))./v.^4;
s4 = #(v) (a10 + (a11/tr^2) + (a12/tr^3))./v.^5;
s5 = #(v) (a13./(tr^3.*v.^2)).*(a14 + (a15./v.^2)).*exp(-a15./v.^2);
y = #(v) -(pr*v./tr) + 1 + s1(v) + s2(v) + s3(v) + s4(v) + s5(v);
root = fzero(y, [1 5]);
% Visualization
fplot(y, [1 5]); hold all; refline(0,0); line([root,root], [-10,30])
I heard a lot about amazing performance of programs written in Haskell, and wanted to make some tests. So, I wrote a 'library' for matrix operations just to compare it's performance with the same stuff written in pure C.
First of all I tested 500000 matrices multiplication performance, and noticed that it was... never-ending (i. e. ending with out of memory exception after 10 minutes of so)! After studying haskell a bit more I managed to get rid of laziness and the best result I managed to get is ~20 times slower than its equivalent in C.
So, the question: could you review the code below and tell if its performance can be improved a bit more? 20 times is still disappointing me a bit.
import Prelude hiding (foldr, foldl, product)
import Data.Monoid
import Data.Foldable
import Text.Printf
import System.CPUTime
import System.Environment
data Vector a = Vec3 a a a
| Vec4 a a a a
deriving Show
instance Foldable Vector where
foldMap f (Vec3 a b c) = f a `mappend` f b `mappend` f c
foldMap f (Vec4 a b c d) = f a `mappend` f b `mappend` f c `mappend` f d
data Matr a = Matr !a !a !a !a
!a !a !a !a
!a !a !a !a
!a !a !a !a
instance Show a => Show (Matr a) where
show m = foldr f [] $ matrRows m
where f a b = show a ++ "\n" ++ b
matrCols (Matr a0 b0 c0 d0 a1 b1 c1 d1 a2 b2 c2 d2 a3 b3 c3 d3)
= [Vec4 a0 a1 a2 a3, Vec4 b0 b1 b2 b3, Vec4 c0 c1 c2 c3, Vec4 d0 d1 d2 d3]
matrRows (Matr a0 b0 c0 d0 a1 b1 c1 d1 a2 b2 c2 d2 a3 b3 c3 d3)
= [Vec4 a0 b0 c0 d0, Vec4 a1 b1 c1 d1, Vec4 a2 b2 c2 d2, Vec4 a3 b3 c3 d3]
matrFromList [a0, b0, c0, d0, a1, b1, c1, d1, a2, b2, c2, d2, a3, b3, c3, d3]
= Matr a0 b0 c0 d0
a1 b1 c1 d1
a2 b2 c2 d2
a3 b3 c3 d3
matrId :: Matr Double
matrId = Matr 1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
normalise (Vec4 x y z w) = Vec4 (x/w) (y/w) (z/w) 1
mult a b = matrFromList [f r c | r <- matrRows a, c <- matrCols b] where
f a b = foldr (+) 0 $ zipWith (*) (toList a) (toList b)
First, I doubt that you'll ever get stellar performance with this implementation. There are too many conversions between different representations. You'd be better off basing your code on something like the vector package. Also you don't provide all your testing code, so there are probably other issues that we can't here. This is because the pipeline of production to consumption has a big impact on Haskell performance, and you haven't provided either end.
Now, two specific problems:
1) Your vector is defined as either a 3 or 4 element vector. This means that for every vector there's an extra check to see how many elements are in use. In C, I imagine your implementation is probably closer to
struct vec {
double *vec;
int length;
}
You should do something similar in Haskell; this is how vector and bytestring are implemented for example.
Even if you don't change the Vector definition, make the fields strict. You should also either add UNPACK pragmas (to Vector and Matrix) or compile with -funbox-strict-fields.
2) Change mult to
mult a b = matrFromList [f r c | r <- matrRows a, c <- matrCols b] where
f a b = Data.List.foldl' (+) 0 $ zipWith (*) (toList a) (toList b)
The extra strictness of foldl' will give much better performance in this case than foldr.
This change alone might make a big difference, but without seeing the rest of your code it's difficult to say.
Answering my own question just to share new results I got yesterday:
I upgraded ghc to the most recent version and performance became indeed not that bad (only ~7 times worse).
Also I tried implementing the matrix in a stupid and simple way (see the listing below) and got really acceptable performance - only about 2 times slower than C equivalent.
data Matr a = Matr ( a, a, a, a
, a, a, a, a
, a, a, a, a
, a, a, a, a)
mult (Matr (!a0, !b0, !c0, !d0,
!a1, !b1, !c1, !d1,
!a2, !b2, !c2, !d2,
!a3, !b3, !c3, !d3))
(Matr (!a0', !b0', !c0', !d0',
!a1', !b1', !c1', !d1',
!a2', !b2', !c2', !d2',
!a3', !b3', !c3', !d3'))
= Matr ( a0'', b0'', c0'', d0''
, a1'', b1'', c1'', d1''
, a2'', b2'', c2'', d2''
, a3'', b3'', c3'', d3'')
where a0'' = a0 * a0' + b0 * a1' + c0 * a2' + d0 * a3'
b0'' = a0 * b0' + b0 * b1' + c0 * b2' + d0 * b3'
c0'' = a0 * c0' + b0 * c1' + c0 * c2' + d0 * c3'
d0'' = a0 * d0' + b0 * d1' + c0 * d2' + d0 * d3'
a1'' = a1 * a0' + b1 * a1' + c1 * a2' + d1 * a3'
b1'' = a1 * b0' + b1 * b1' + c1 * b2' + d1 * b3'
c1'' = a1 * c0' + b1 * c1' + c1 * c2' + d1 * c3'
d1'' = a1 * d0' + b1 * d1' + c1 * d2' + d1 * d3'
a2'' = a2 * a0' + b2 * a1' + c2 * a2' + d2 * a3'
b2'' = a2 * b0' + b2 * b1' + c2 * b2' + d2 * b3'
c2'' = a2 * c0' + b2 * c1' + c2 * c2' + d2 * c3'
d2'' = a2 * d0' + b2 * d1' + c2 * d2' + d2 * d3'
a3'' = a3 * a0' + b3 * a1' + c3 * a2' + d3 * a3'
b3'' = a3 * b0' + b3 * b1' + c3 * b2' + d3 * b3'
c3'' = a3 * c0' + b3 * c1' + c3 * c2' + d3 * c3'
d3'' = a3 * d0' + b3 * d1' + c3 * d2' + d3 * d3'