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'm not much familiar with this programming language and I just need to run one function to compute some coeficients.
f[x] = x^2 - 2 x + 2
g[x] = x^3 - 2 x^2 - 2 x - 2
f1 = Root[f[x], 1];
f2 = Root[f[x], 2];
g1 = Root[g[x], 1];
g2 = Root[g[x], 2];
g3 = Root[g[x], 3];
foo[rootList, alpha, beta] :=
(
res = {};
For[i = 1, i <= Length[rootList], i++, alphaI = rootList[[i]];
For[j = 1, j <= Length[rootList], j++, betaJ = rootList[[j]];
If[betaJ != beta,
(
kor = Simplify [(alphaI - alpha) / (beta - betaJ)];
res = Append[res, N[kor, 5]];
),
]
]
]
Return[res];
)
roots = [f1, f2, g1, g2, g3];
cs = foo[roots, f1, g1]
this piece of code gives me this error:
Syntax::tsntxi: "For[i=1,i<=Length[rootList],i++,alphaI=rootList[[i]];" is incomplete; more input is needed.
And don't see what is wrong. I'm using mathematica 10.4
Fixing the syntax errors.
f[x_] := x^2 - 2 x + 2
g[x_] := x^3 - 2 x^2 - 2 x - 2
f1 = Root[f[x], 1];
f2 = Root[f[x], 2];
g1 = Root[g[x], 1];
g2 = Root[g[x], 2];
g3 = Root[g[x], 3];
foo[rootList_, alpha_, beta_] :=
(
res = {};
For[i = 1, i <= Length[rootList], i++, alphaI = rootList[[i]];
For[j = 1, j <= Length[rootList], j++, betaJ = rootList[[j]];
If[betaJ != beta,
(
kor = Simplify[(alphaI - alpha)/(beta - betaJ)];
res = Append[res, N[kor, 5]];
)
]
]
];
res
)
roots = {f1, f2, g1, g2, g3};
cs = foo[roots, f1, g1]
I have implemented a code for image warping using bilinear interpolation:
Matlab image rotation
I would like to improve the code by using bicubic interpolation to rotate the image WITHOUT using the built-in functions like imrotate or imwarp and interp functions in MATLAB.
I successfully managed to implement a full working example.
Code is based on Anna1994's code: Matlab image rotation
Biqubic code is also based on Java (and C++) implementation posted here: http://www.paulinternet.nl/?page=bicubic
The following code applies image rotation example using biqubic interpolation:
function BicubicInterpolationTest()
close all;
% clear all;
img = 'cameraman.tif';
input_image =double(imread(img))./255;
H=size(input_image,1); % height
W=size(input_image,2); % width
th=120*pi/180; %Rotate 120 degrees
s0 = 2;
s1 = 2;
x0 = -W/2;
x1 = -H/2;
T=[1 0 x0 ; ...
0 1 x1 ; ...
0 0 1];
RST = [ (s0*cos(th)) (-s1*sin(th)) ((s0*x0*cos(th))-(s1*x1*sin(th))); ...
(s0*sin(th)) (s1*cos(th)) ((s0*x0*sin(th))+(s1*x1*cos(th))); ...
0 0 1];
M=inv(T)*RST;
N = inv(M);
output_image=zeros(H,W,size(input_image,3));
for i=1:W
for j=1:H
x = [i ; j ; 1];
y = N * x;
a = y(1)/y(3);
b = y(2)/y(3);
%Nearest neighbor
% a = round(a);
% b = round(b);
%Bilinear interpolation (applies RGB image):
% x1 = floor(a);
% y1 = floor(b);
% x2 = x1 + 1;
% y2 = y1 + 1;
% if ((x1 >= 1) && (y1 >= 1) && (x2 <= W) && (y2 <= H))
% %Load 2x2 pixels
% i11 = input_image(y1, x1, :); %Top left pixel
% i21 = input_image(y2, x1, :); %Bottom left pixel
% i12 = input_image(y1, x2, :); %Top right pixel
% i22 = input_image(y2, x2, :); %Bottom right pixel
%
% %Interpolation wieghts
% dx = x2 - a;
% dy = y2 - b;
%
% %Bi-lienar interpolation
% output_image(j, i, :) = i11*dx*dy + i21*dx*(1-dy) + i12*(1-dx)*dy + i22*(1-dx)*(1-dy);
% end
x1 = floor(a);
y1 = floor(b);
%Bicubic interpolation (applies grayscale image)
if ((x1 >= 2) && (y1 >= 2) && (x1 <= W-2) && (y1 <= H-2))
%Load 4x4 pixels
P = input_image(y1-1:y1+2, x1-1:x1+2);
%Interpolation wieghts
dx = a - x1;
dy = b - y1;
%Bi-bicubic interpolation
output_image(j, i) = bicubicInterpolate(P, dx, dy);
end
end
end
imshow(output_image);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Verify implementation by comparing with Matalb build in function imwarp:
tform = affine2d(M');
ref_image = imwarp(input_image, tform, 'OutputView', imref2d(size(input_image)), 'Interp', 'cubic');
figure;imshow(ref_image)
figure;imshow(output_image - ref_image)
max_diff = max(abs(output_image(:) - ref_image(:)));
disp(['Maximum difference from imwarp = ', num2str(max_diff)]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%http://www.paulinternet.nl/?page=bicubic
%double cubicInterpolate (double p[4], double x) {
% return p[1] + 0.5 * x*(p[2] - p[0] + x*(2.0*p[0] - 5.0*p[1] + 4.0*p[2] - p[3] + x*(3.0*(p[1] - p[2]) + p[3] - p[0])));
%}
function q = cubicInterpolate(p, x)
q = p(2) + 0.5 * x*(p(3) - p(1) + x*(2.0*p(1) - 5.0*p(2) + 4.0*p(3) - p(4) + x*(3.0*(p(2) - p(3)) + p(4) - p(1))));
%http://www.paulinternet.nl/?page=bicubic
% double bicubicInterpolate (double p[4][4], double x, double y) {
% double arr[4];
% arr[0] = cubicInterpolate(p[0], y);
% arr[1] = cubicInterpolate(p[1], y);
% arr[2] = cubicInterpolate(p[2], y);
% arr[3] = cubicInterpolate(p[3], y);
% return cubicInterpolate(arr, x);
% }
function q = bicubicInterpolate(p, x, y)
q1 = cubicInterpolate(p(1,:), x);
q2 = cubicInterpolate(p(2,:), x);
q3 = cubicInterpolate(p(3,:), x);
q4 = cubicInterpolate(p(4,:), x);
q = cubicInterpolate([q1, q2, q3, q4], y);
I verified implementation by comparing to Matalb build in function imwarp
Result:
The following example uses the "CachedBicubicInterpolator" code version, and also supports RGB image:
function BicubicInterpolationTest2()
close all;
% clear all;
img = 'peppers.png';
input_image = double(imread(img))./255;
H=size(input_image,1); % height
W=size(input_image,2); % width
th=120*pi/180; %Rotate 120 degrees
s0 = 0.8;
s1 = 0.8;
x0 = -W/2;
x1 = -H/2;
T=[1 0 x0 ; ...
0 1 x1 ; ...
0 0 1];
RST = [ (s0*cos(th)) (-s1*sin(th)) ((s0*x0*cos(th))-(s1*x1*sin(th))); ...
(s0*sin(th)) (s1*cos(th)) ((s0*x0*sin(th))+(s1*x1*cos(th))); ...
0 0 1];
M=inv(T)*RST;
N = inv(M);
output_image=zeros(H,W,size(input_image,3));
for i=1:W
for j=1:H
x = [i ; j ; 1];
y = N * x;
a = y(1)/y(3);
b = y(2)/y(3);
x1 = floor(a);
y1 = floor(b);
%Bicubic interpolation (applies grayscale image)
if ((x1 >= 2) && (y1 >= 2) && (x1 <= W-2) && (y1 <= H-2))
%Load 4x4 pixels
P = input_image(y1-1:y1+2, x1-1:x1+2, :);
%Interpolation wieghts
dx = a - x1;
dy = b - y1;
%Bi-bicubic interpolation
output_image(j, i, :) = bicubicInterpolate(P, dx, dy);
end
end
end
imshow(output_image);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Verify implementation by comparing with Matalb build in function imwarp:
tform = affine2d(M');
ref_image = imwarp(input_image, tform, 'OutputView', imref2d(size(input_image)), 'Interp', 'cubic');
figure;imshow(ref_image)
figure;imshow(abs(output_image - ref_image), []);impixelinfo
max_diff = max(abs(output_image(:) - ref_image(:)));
disp(['Maximum difference from imwarp = ', num2str(max_diff)]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [p0, p1, p2, p3] = list4(P)
P = squeeze(P);
p0 = P(1, :);
p1 = P(2, :);
p2 = P(3, :);
p3 = P(4, :);
%http://www.paulinternet.nl/?page=bicubic
% public void updateCoefficients (double[][] p) {
% a00 = p[1][1];
% a01 = -.5*p[1][0] + .5*p[1][2];
% a02 = p[1][0] - 2.5*p[1][1] + 2*p[1][2] - .5*p[1][3];
% a03 = -.5*p[1][0] + 1.5*p[1][1] - 1.5*p[1][2] + .5*p[1][3];
% a10 = -.5*p[0][1] + .5*p[2][1];
% a11 = .25*p[0][0] - .25*p[0][2] - .25*p[2][0] + .25*p[2][2];
% a12 = -.5*p[0][0] + 1.25*p[0][1] - p[0][2] + .25*p[0][3] + .5*p[2][0] - 1.25*p[2][1] + p[2][2] - .25*p[2][3];
% a13 = .25*p[0][0] - .75*p[0][1] + .75*p[0][2] - .25*p[0][3] - .25*p[2][0] + .75*p[2][1] - .75*p[2][2] + .25*p[2][3];
% a20 = p[0][1] - 2.5*p[1][1] + 2*p[2][1] - .5*p[3][1];
% a21 = -.5*p[0][0] + .5*p[0][2] + 1.25*p[1][0] - 1.25*p[1][2] - p[2][0] + p[2][2] + .25*p[3][0] - .25*p[3][2];
% a22 = p[0][0] - 2.5*p[0][1] + 2*p[0][2] - .5*p[0][3] - 2.5*p[1][0] + 6.25*p[1][1] - 5*p[1][2] + 1.25*p[1][3] + 2*p[2][0] - 5*p[2][1] + 4*p[2][2] - p[2][3] - .5*p[3][0] + 1.25*p[3][1] - p[3][2] + .25*p[3][3];
% a23 = -.5*p[0][0] + 1.5*p[0][1] - 1.5*p[0][2] + .5*p[0][3] + 1.25*p[1][0] - 3.75*p[1][1] + 3.75*p[1][2] - 1.25*p[1][3] - p[2][0] + 3*p[2][1] - 3*p[2][2] + p[2][3] + .25*p[3][0] - .75*p[3][1] + .75*p[3][2] - .25*p[3][3];
% a30 = -.5*p[0][1] + 1.5*p[1][1] - 1.5*p[2][1] + .5*p[3][1];
% a31 = .25*p[0][0] - .25*p[0][2] - .75*p[1][0] + .75*p[1][2] + .75*p[2][0] - .75*p[2][2] - .25*p[3][0] + .25*p[3][2];
% a32 = -.5*p[0][0] + 1.25*p[0][1] - p[0][2] + .25*p[0][3] + 1.5*p[1][0] - 3.75*p[1][1] + 3*p[1][2] - .75*p[1][3] - 1.5*p[2][0] + 3.75*p[2][1] - 3*p[2][2] + .75*p[2][3] + .5*p[3][0] - 1.25*p[3][1] + p[3][2] - .25*p[3][3];
% a33 = .25*p[0][0] - .75*p[0][1] + .75*p[0][2] - .25*p[0][3] - .75*p[1][0] + 2.25*p[1][1] - 2.25*p[1][2] + .75*p[1][3] + .75*p[2][0] - 2.25*p[2][1] + 2.25*p[2][2] - .75*p[2][3] - .25*p[3][0] + .75*p[3][1] - .75*p[3][2] + .25*p[3][3];
% }
% public double getValue (double x, double y) {
% double x2 = x * x;
% double x3 = x2 * x;
% double y2 = y * y;
% double y3 = y2 * y;
%
% return (a00 + a01 * y + a02 * y2 + a03 * y3) +
% (a10 + a11 * y + a12 * y2 + a13 * y3) * x +
% (a20 + a21 * y + a22 * y2 + a23 * y3) * x2 +
% (a30 + a31 * y + a32 * y2 + a33 * y3) * x3;
% }
function q = bicubicInterpolate(P, x, y)
[p00, p01, p02, p03] = list4(P(1, :, :));
[p10, p11, p12, p13] = list4(P(2, :, :));
[p20, p21, p22, p23] = list4(P(3, :, :));
[p30, p31, p32, p33] = list4(P(4, :, :));
a00 = p11;
a01 = -.5*p10 + .5*p12;
a02 = p10 - 2.5*p11 + 2*p12 - .5*p13;
a03 = -.5*p10 + 1.5*p11 - 1.5*p12 + .5*p13;
a10 = -.5*p01 + .5*p21;
a11 = .25*p00 - .25*p02 - .25*p20 + .25*p22;
a12 = -.5*p00 + 1.25*p01 - p02 + .25*p03 + .5*p20 - 1.25*p21 + p22 - .25*p23;
a13 = .25*p00 - .75*p01 + .75*p02 - .25*p03 - .25*p20 + .75*p21 - .75*p22 + .25*p23;
a20 = p01 - 2.5*p11 + 2*p21 - .5*p31;
a21 = -.5*p00 + .5*p02 + 1.25*p10 - 1.25*p12 - p20 + p22 + .25*p30 - .25*p32;
a22 = p00 - 2.5*p01 + 2*p02 - .5*p03 - 2.5*p10 + 6.25*p11 - 5*p12 + 1.25*p13 + 2*p20 - 5*p21 + 4*p22 - p23 - .5*p30 + 1.25*p31 - p32 + .25*p33;
a23 = -.5*p00 + 1.5*p01 - 1.5*p02 + .5*p03 + 1.25*p10 - 3.75*p11 + 3.75*p12 - 1.25*p13 - p20 + 3*p21 - 3*p22 + p23 + .25*p30 - .75*p31 + .75*p32 - .25*p33;
a30 = -.5*p01 + 1.5*p11 - 1.5*p21 + .5*p31;
a31 = .25*p00 - .25*p02 - .75*p10 + .75*p12 + .75*p20 - .75*p22 - .25*p30 + .25*p32;
a32 = -.5*p00 + 1.25*p01 - p02 + .25*p03 + 1.5*p10 - 3.75*p11 + 3*p12 - .75*p13 - 1.5*p20 + 3.75*p21 - 3*p22 + .75*p23 + .5*p30 - 1.25*p31 + p32 - .25*p33;
a33 = .25*p00 - .75*p01 + .75*p02 - .25*p03 - .75*p10 + 2.25*p11 - 2.25*p12 + .75*p13 + .75*p20 - 2.25*p21 + 2.25*p22 - .75*p23 - .25*p30 + .75*p31 - .75*p32 + .25*p33;
x2 = x * x;
x3 = x2 * x;
y2 = y * y;
y3 = y2 * y;
% q = (a00 + a01 * y + a02 * y2 + a03 * y3) +...
% (a10 + a11 * y + a12 * y2 + a13 * y3) * x +...
% (a20 + a21 * y + a22 * y2 + a23 * y3) * x2 +...
% (a30 + a31 * y + a32 * y2 + a33 * y3) * x3;
q = (a00 + a01 * x + a02 * x2 + a03 * x3) +...
(a10 + a11 * x + a12 * x2 + a13 * x3) * y +...
(a20 + a21 * x + a22 * x2 + a23 * x3) * y2 +...
(a30 + a31 * x + a32 * x2 + a33 * x3) * y3;
Result:
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])