You didn't include the functions where the error occurs. So how should we be able to help ?
For such a long program, it's necessary that you provide executable code that reproduces the error.
lsqnonlin - error in finite differences, arrays of incompatible size
2 views (last 30 days)
Show older comments
I am trying to use lsqnonlin as a batch processor
This is the code I wrote (relevant parameters such as xx0_1_ok, T, rho_meas, t_eval and such are defined in previous portion of the code)
%% Batch processor
% Initial conditions
xx0_1_noise = xx0_1_ok + randn(size(xx0_1_ok)).*0.01.*xx0_1_ok;
xx0_2_noise = xx0_2_ok + randn(size(xx0_2_ok)).*0.01.*xx0_2_ok;
x_hat = lsqnonlin(@(x) batch_fun(x, T, rho_meas, t_eval), [xx0_1_noise xx0_2_noise]);
function F = batch_fun(x, T, rho_meas, t_eval_rho)
LU = astroConstants(7); %Transform from [LU] to [km]
TU = 2 * pi * 27.321661 * 24 * 60 * 60; %Transform from [TU] to [s]
options = odeset('Reltol',1.e-13,'Abstol',1.e-20);
% Propagate
sol1 = ode113(@CRTBP, [0 T], x(1:6), options);
t_1 = sol1.x;
sol2 = ode113(@CRTBP, [0 T], x(7:12), options);
t_2 = sol2.x;
% Evaluate orbits at same instants of time
if length(t_1) > length(t_2)
t_eval = t_1;
x2 = deval(sol2, t_eval);
x2(1:3,:) = x2(1:3,:) * LU;
x2(4:6, :) = x2(4:6,:) * LU / TU;
x1(1:3,:) = sol1.y(1:3,:) * LU;
x1(4:6,:) = sol1.y(4:6,:) * LU / TU;
else
t_eval = t_2;
x1 = deval(sol1, t_eval);
x1(1:3,:) = x1(1:3,:) * LU;
x1(4:6, :) = x1(4:6,:) * LU / TU;
x2(1:3,:) = sol2.y(1:3,:) * LU;
x2(4:6,:) = sol2.y(4:6,:) * LU / TU;
end
% Interpolate measurements at instants of time of iteration
rho_meas_iter = interp1(t_eval_rho, rho_meas, t_eval, "linear");
F = sqrt(dot(x1(1:3,:)-x2(1:3,:), x1(1:3,:)-x2(1:3,:))) - rho_meas_iter;
end
function [xx0_out, T0_out] = halo(xx0, T0)
%--------------------------------------------------------------------------
% Finds initial conditions for CRTBP in the Earth-Moon system
% Prototype: [xx0_out, T0_out] = halo(xx0, T0)
% Input:
% xx0 - [6x1] initial guess in position and velocity of orbit
% T0 - initial guess for period of orbit
% Output:
% xx0_out - [6x1] correct initial conditions in position and velocity of orbit
% T0 - correct period of orbit
%--------------------------------------------------------------------------
%% Initialization
eps = 1e-10;
iter = 1;
Fnorm = 1;
mu = 0.012150584269940;
%Set initial conditions for first integration
% Exact guess
% xx0 = [0.987582486032319, 0, 0.005261320588633, 0, 2.123290002593113, 0]; %NRHO
% T0 = 1.395647230407869;
% xx0 = [1.097286086652747, 0, 0.053070202898073, 0, 0.241115522202103, 0]; %Halo
% T0 = 3.358019851591808;
% xx0 = [0.434372438333557, 0, 0, 0, 1.423278979058999, 0]; %DRO
% T0 = 6.095332268294693;
% xx0 = [1.076513567928299, 0, 0, 0, 0.390404931659456, 0]; %Lyapunov
% T0 = 3.604707326612602;
I = eye(6);
x0 = [xx0 I(1,:) I(2,:) I(3,:) I(4,:) I(5,:) I(6,:)]';
%% Refine initial condition
while Fnorm > eps && iter < 10
tspan = [0 T0/2];
options = odeset('Reltol',1.e-13,'Abstol',1.e-20);
[~, state] = ode45(@CRTBP_STM, tspan, x0, options);
x = state(end, 1:6)';
STM = [state(end, 7:12); state(end, 13:18); state(end, 19:24); state(end, 25:30); state(end, 31:36); state(end, 37:42)];
F = [x(2); x(4); x(6)];
Fnorm = norm(F);
r1 = sqrt((x(1) + mu)^2 + x(2)^2 +x(3)^2);
r2 = sqrt((x(1) + mu - 1)^2 + x(2)^2 + x(3)^2);
ddx = x(1) - (1 - mu) * (x(1) + mu) / r1^3 - mu ...
* (x(1) + mu - 1) / r2^3 + 2 * x(5);
ddz = ((mu - 1) / r1^3 - mu / r2^3) * x(3);
DF = [STM(2,1) STM(2,5) STM(2,6) x(5);
STM(4,1) STM(4,5) STM(4,6) ddx;
STM(6,1) STM(6,5) STM(6,6) ddz];
%Correct
Xold = [x0(1) x0(5) x0(6) T0]';
invDF = (DF * DF') \ F;
Xnew = Xold - DF' * invDF;
x0(1) = Xnew(1);
x0(5) = Xnew(2);
x0(6) = Xnew(3);
T0 = Xnew(4);
iter = iter + 1;
end
xx0_out = x0(1:6);
T0_out = T0;
end
lsqnonlin seems to do a few iterations of the objective function F however in 2 or 3 iterations it returns the following error
Arrays have incompatible sizes for this operation.
Error in finitedifferences
Error in computeFinDiffGradAndJac
Error in sfdnls (line 54)
computeFinDiffGradAndJac(x,funfcn,confcn,valx, ...
Error in snls (line 165)
[A,findiffevals] = sfdnls(xcurr,fvec,Jstr,group,alpha,funfcn,l,u,...
Error in lsqncommon (line 179)
snls(funfcn,xC,lb,ub,flags.verbosity,options,defaultopt,initVals.F,initVals.J,caller, ...
Error in lsqnonlin (line 260)
lsqncommon(funfcn,xCurrent,lb,ub,options,defaultopt,optimgetFlag,caller,...
Error in main (line 83)
x_hat = lsqnonlin(@(x) batch_fun(x, T, rho_meas, t_eval), [xx0_1_noise xx0_2_noise]);
Can you help me find out why? Thanks!
2 Comments
Astrid Barzaghi
on 17 Jun 2023
Edited: Torsten
on 17 Jun 2023
I'm sorry, I'll provide the whole code here. Thank you!
%% Initialization
close all
clear
clc
%% Propagate orbits
mu = 0.012150584269940;
LU = 3.844e5; %Transform from [LU] to [km]
TU = 2 * pi * 27.321661 * 24 * 60 * 60; %Transform from [TU] to [s]
earth_coord = [-mu * LU 0 0];
moon_coord = [(1-mu) * LU 0 0];
options = odeset('Reltol',1.e-13,'Abstol',1.e-20);
% Find IC orbit 1 (Halo orbit)
xx0_1 = [1.09728608665, 0, 0.05307020289, 0, 0.24111552220, 0];
T0_1 = 3.35801985159;
[xx0_1_ok, T0_1_ok] = halo(xx0_1, T0_1);
% Find IC orbit 2 (DRO orbit)
xx0_2 = [0.43437243833, 0, 0, 0, 1.423278979058, 0];
T0_2 = 6.095332268294;
[xx0_2_ok, T0_2_ok] = halo(xx0_2, T0_2);
% Propagate
T = max([T0_2_ok T0_1_ok]);
sol1 = ode113(@CRTBP, [0 T], xx0_1_ok, options);
t_1 = sol1.x;
sol2 = ode113(@CRTBP, [0 T], xx0_2_ok, options);
t_2 = sol2.x;
% Evaluate orbits at same instants of time
if length(t_1) > length(t_2)
t_eval = t_1;
x_2 = deval(sol2, t_eval);
x_2(1:3,:) = x_2(1:3,:) * LU;
x_2(4:6, :) = x_2(4:6,:) * LU / TU;
x_1(1:3,:) = sol1.y(1:3,:) * LU;
x_1(4:6,:) = sol1.y(4:6,:) * LU / TU;
t_2 = t_eval * TU;
t_1 = t_1 * TU;
else
t_eval = t_2;
x_1 = deval(sol1, t_eval);
x_1(1:3,:) = x_1(1:3,:) * LU;
x_1(4:6, :) = x_1(4:6,:) * LU / TU;
x_2(1:3,:) = sol2.y(1:3,:) * LU;
x_2(4:6,:) = sol2.y(4:6,:) * LU / TU;
t_1 = t_eval * TU;
t_2 = t_2 * TU;
end
% Plot orbits
figure
plot3(x_1(1,:), x_1(2,:), x_1(3,:), 'g', 'LineWidth',2)
hold on
plot3(x_2(1,:), x_2(2,:), x_2(3,:), 'm', 'LineWidth', 2)
moon_size = 100;
moon_color = 'k';
moon_marker = 'o';
eart_size = 200; % Size of the moon symbol
earth_color = 'b'; % Color of the moon symbol
earth_marker = 'o'; % Marker shape for the moon symbol
scatter3(earth_coord(1), earth_coord(2), earth_coord(3), eart_size, earth_color, earth_marker, 'filled')
scatter3(moon_coord(1), moon_coord(2), moon_coord(3), moon_size, moon_color, moon_marker, 'filled')
legend('Halo orbit', 'DRO orbit', 'Earth', 'Moon')
xlabel('x')
ylabel('y')
zlabel('z')
%% Generate measurements
rho = meas_model(x_1, x_2);
std_rho = 1e-3; %[km]
noise = std_rho * randn(size(rho));
rho_meas = rho + noise;
%% Batch processor
% Initial conditions
xx0_1_noise = xx0_1_ok + randn(size(xx0_1_ok)).*0.01.*xx0_1_ok;
xx0_2_noise = xx0_2_ok + randn(size(xx0_2_ok)).*0.01.*xx0_2_ok;
x_hat = lsqnonlin(@(x) batch_fun(x, T, rho_meas, t_eval), [xx0_1_noise xx0_2_noise]);
function dx_vect = CRTBP_STM(~, x_vect)
%--------------------------------------------------------------------------
% System of ODEs describing the full dynamic of CRTBP motion (state and
% STM)
% Function prototype: dx_vect = CRTBP_STM(t, x_vect)
% Input:
% x_vect - 42x1 vector containing position, velocity and STM elements by
% row
% Output:
% dx_vect - 42x1 vector of EoMS in CRTBP
%--------------------------------------------------------------------------
% Initialization
dx_vect = zeros(42,1);
mu = 0.012150584269940;
%---------------------------------State------------------------------------
x = x_vect(1); %rx
y = x_vect(2); %ry
z = x_vect(3); %rz
vx = x_vect(4); %vx
vy = x_vect(5); %vy
vz = x_vect(6); %vz
r1 = sqrt((x + mu)^2 + y^2 +z^2);
r2 = sqrt((x + mu - 1)^2 + y^2 + z^2);
%--------------------------------STM---------------------------------------
%Matrix phi
phi1 = x_vect(7:12);
phi2 = x_vect(13:18);
phi3 = x_vect(19:24);
phi4 = x_vect(25:30);
phi5 = x_vect(31:36);
phi6 = x_vect(37:42);
phi = [phi1 phi2 phi3 phi4 phi5 phi6];
%Matrix A
O = zeros(3);
I = eye(3);
%Uxx1
Uxx1dx = 1 - (1-mu) * (((x+mu)^2 + y^2 + z^2)^(-3/2) +...
-3*(x+mu)^2*(((x+mu)^2 + y^2 + z^2))^(-5/2)) +...
- mu * (((x+mu-1)^2 + y^2 + z^2)^(-3/2) +...
-3 * (x+mu-1)^2*(((x+mu-1)^2 + y^2 + z^2))^(-5/2));
Uxx1dy = 3*(1-mu) * y * (x+mu) * ((x+mu)^2 + y^2 + z^2)^(-5/2) +...
+ 3* mu * y * (x+mu-1) * ((x+mu-1)^2 + y^2 + z^2)^(-5/2);
Uxx1dz = 3*(1-mu) * z * (x+mu) * ((x+mu)^2 + y^2 + z^2)^(-5/2) +...
+ 3* mu * z * (x+mu-1) * ((x+mu-1)^2 + y^2 + z^2)^(-5/2);
%Uxx2
Uxx2dx = Uxx1dy;
Uxx2dy = 1 - (1-mu) * (((x+mu)^2 + y^2 + z^2)^(-3/2) +...
-3*y^2*(((x+mu)^2 + y^2 + z^2))^(-5/2)) +...
- mu * (((x+mu-1)^2 + y^2 + z^2)^(-3/2) +...
-3 * y^2*(((x+mu-1)^2 + y^2 + z^2))^(-5/2));
Uxx2dz = 3*(1-mu) * z * y * ((x+mu)^2 + y^2 + z^2)^(-5/2) +...
+ 3* mu * z * y * ((x+mu-1)^2 + y^2 + z^2)^(-5/2);
%Uxx3
Uxx3dx = Uxx1dz;
Uxx3dy = Uxx2dz;
Uxx3dz = 0 - (1-mu) * (((x+mu)^2 + y^2 + z^2)^(-3/2) +...
-3 * z^2*(((x+mu)^2 + y^2 + z^2))^(-5/2)) +...
- mu * (((x+mu-1)^2 + y^2 + z^2)^(-3/2) +...
-3 * z^2*(((x+mu-1)^2 + y^2 + z^2))^(-5/2));
%SubMatricies
Uxx = [Uxx1dx, Uxx1dy, Uxx1dz;
Uxx2dx, Uxx2dy, Uxx2dz;
Uxx3dx, Uxx3dy, Uxx3dz];
H = [0, 2, 0;
-2, 0, 0;
0, 0, 0];
%State Matrix
A = [O I;
Uxx H];
phidot = A * phi;
phidot1 = phidot(1,:)';
phidot2 = phidot(2,:)';
phidot3 = phidot(3,:)';
phidot4 = phidot(4,:)';
phidot5 = phidot(5,:)';
phidot6 = phidot(6,:)';
%---------------------------------ODEs------------------------------------
dx_vect(1) = vx; %dx
dx_vect(2) = vy; %dy
dx_vect(3) = vz; %dz
dx_vect(4) = x - (1 - mu) * (x + mu) / r1^3 - mu ...
* (x + mu - 1) / r2^3 + 2 * vy; %ddx
dx_vect(5) = (1 - (1 - mu) / r1^3 - mu / r2^3) * y - 2 * vx; %ddy
dx_vect(6) = ((mu - 1) / r1^3 - mu / r2^3) * z; %ddz
dx_vect(7:12) = phidot1;
dx_vect(13:18) = phidot2;
dx_vect(19:24) = phidot3;
dx_vect(25:30) = phidot4;
dx_vect(31:36) = phidot5;
dx_vect(37:42) = phidot6;
end
function dx_vect = CRTBP(~, x_vect)
%--------------------------------------------------------------------------
% System of ODEs describing the EoMs in the CRTBP
% Header: dx_vect = CRTBP(t, x_vect)
% Input:
% x_vect - 6x1 vector containing position and velocity coordinates
% Output:
% dx_vect - 6x1 vector of EoMS in CRTBP
%--------------------------------------------------------------------------
mu3B = 0.012150584269940;
dx_vect = zeros(6,1);
x = x_vect(1); %rx
y = x_vect(2); %ry
z = x_vect(3); %rz
vx = x_vect(4); %vx
vy = x_vect(5); %vy
vz = x_vect(6); %vz
r1 = sqrt((x + mu3B)^2 + y^2 +z^2);
r2 = sqrt((x + mu3B - 1)^2 + y^2 + z^2);
dx_vect(1) = vx; %dx
dx_vect(2) = vy; %dy
dx_vect(3) = vz; %dz
dx_vect(4) = x - (1 - mu3B) * (x + mu3B) / r1^3 - mu3B ...
* (x + mu3B - 1) / r2^3 + 2 * vy; %ddx
dx_vect(5) = (1 - (1 - mu3B) / r1^3 - mu3B / r2^3) * y - 2 * vx; %ddy
dx_vect(6) = ((mu3B - 1) / r1^3 - mu3B / r2^3) * z; %ddz
end
function [xx0_out, T0_out] = halo(xx0, T0)
%--------------------------------------------------------------------------
% Finds initial conditions for CRTBP in the Earth-Moon system
% Prototype: [xx0_out, T0_out] = halo(xx0, T0)
% Input:
% xx0 - [6x1] initial guess in position and velocity of orbit
% T0 - initial guess for period of orbit
% Output:
% xx0_out - [6x1] correct initial conditions in position and velocity of orbit
% T0 - correct period of orbit
%--------------------------------------------------------------------------
%% Initialization
eps = 1e-10;
iter = 1;
Fnorm = 1;
mu = 0.012150584269940;
%Set initial conditions for first integration
% Exact guess
% xx0 = [0.987582486032319, 0, 0.005261320588633, 0, 2.123290002593113, 0]; %NRHO
% T0 = 1.395647230407869;
% xx0 = [1.097286086652747, 0, 0.053070202898073, 0, 0.241115522202103, 0]; %Halo
% T0 = 3.358019851591808;
% xx0 = [0.434372438333557, 0, 0, 0, 1.423278979058999, 0]; %DRO
% T0 = 6.095332268294693;
% xx0 = [1.076513567928299, 0, 0, 0, 0.390404931659456, 0]; %Lyapunov
% T0 = 3.604707326612602;
I = eye(6);
x0 = [xx0 I(1,:) I(2,:) I(3,:) I(4,:) I(5,:) I(6,:)]';
%% Refine initial condition
while Fnorm > eps && iter < 10
tspan = [0 T0/2];
options = odeset('Reltol',1.e-13,'Abstol',1.e-20);
[~, state] = ode45(@CRTBP_STM, tspan, x0, options);
x = state(end, 1:6)';
STM = [state(end, 7:12); state(end, 13:18); state(end, 19:24); state(end, 25:30); state(end, 31:36); state(end, 37:42)];
F = [x(2); x(4); x(6)];
Fnorm = norm(F);
r1 = sqrt((x(1) + mu)^2 + x(2)^2 +x(3)^2);
r2 = sqrt((x(1) + mu - 1)^2 + x(2)^2 + x(3)^2);
ddx = x(1) - (1 - mu) * (x(1) + mu) / r1^3 - mu ...
* (x(1) + mu - 1) / r2^3 + 2 * x(5);
ddz = ((mu - 1) / r1^3 - mu / r2^3) * x(3);
DF = [STM(2,1) STM(2,5) STM(2,6) x(5);
STM(4,1) STM(4,5) STM(4,6) ddx;
STM(6,1) STM(6,5) STM(6,6) ddz];
%Correct
Xold = [x0(1) x0(5) x0(6) T0]';
invDF = (DF * DF') \ F;
Xnew = Xold - DF' * invDF;
x0(1) = Xnew(1);
x0(5) = Xnew(2);
x0(6) = Xnew(3);
T0 = Xnew(4);
iter = iter + 1;
end
xx0_out = x0(1:6);
T0_out = T0;
end
function x_meas = meas_model(x1, x2)
%--------------------------------------------------------------------------
% Measurement model
% Prototype: x_meas = meas_model(x)
% Input:
% x1 - [nx6] [km, km/s] ideal position and velocity of orbit 1 at n instants of time
% x2 - [nx6] [km, km/s] ideal position and velocity of orbit 2 at n instants of time
% Output:
% x_meas - [nxm] measurements
%--------------------------------------------------------------------------
%% Range measurement
x_meas = sqrt(dot(x1(1:3,:)-x2(1:3,:), x1(1:3,:)-x2(1:3,:)));
end
function F = batch_fun(x, T, rho_meas, t_eval_rho)
LU = 3.844e5; %Transform from [LU] to [km]
TU = 2 * pi * 27.321661 * 24 * 60 * 60; %Transform from [TU] to [s]
options = odeset('Reltol',1.e-13,'Abstol',1.e-20);
% Propagate
sol1 = ode113(@CRTBP, [0 T], x(1:6), options);
t_1 = sol1.x;
sol2 = ode113(@CRTBP, [0 T], x(7:12), options);
t_2 = sol2.x;
% Evaluate orbits at same instants of time
if length(t_1) > length(t_2)
t_eval = t_1;
x2 = deval(sol2, t_eval);
x2(1:3,:) = x2(1:3,:) * LU;
x2(4:6, :) = x2(4:6,:) * LU / TU;
x1(1:3,:) = sol1.y(1:3,:) * LU;
x1(4:6,:) = sol1.y(4:6,:) * LU / TU;
else
t_eval = t_2;
x1 = deval(sol1, t_eval);
x1(1:3,:) = x1(1:3,:) * LU;
x1(4:6, :) = x1(4:6,:) * LU / TU;
x2(1:3,:) = sol2.y(1:3,:) * LU;
x2(4:6,:) = sol2.y(4:6,:) * LU / TU;
end
% Interpolate measurements at instants of time of iteration
rho_meas_iter = interp1(t_eval_rho, rho_meas, t_eval, "linear");
F = sqrt(dot(x1(1:3,:)-x2(1:3,:), x1(1:3,:)-x2(1:3,:))) - rho_meas_iter
end
Accepted Answer
Torsten
on 17 Jun 2023
Edited: Torsten
on 17 Jun 2023
The size of your problem changes in the course of the computation from 685 to 650 functions to be minimized (see above). This will make "lsqnonlin" throw an error.
The size of the F-vector returned from "batch_fun" must remain the same.
You shouldn't use interpolation as
% Interpolate measurements at instants of time of iteration
rho_meas_iter = interp1(t_eval_rho, rho_meas, t_eval, "linear");
F = sqrt(dot(x1(1:3,:)-x2(1:3,:), x1(1:3,:)-x2(1:3,:))) - rho_meas_iter
The way to handle the problem is to force the ODE integrator to output the solution exactly at the times specified in your measurement vector t_eval_rho. For this end - instead of using [0 T ] as the tspan in the calls to ode113 - use t_eval_rho. As a consequence, you don't need to use interpolation, but you can explicitly return
F = sqrt(dot(x1(1:3,:)-x2(1:3,:), x1(1:3,:)-x2(1:3,:))) - rho_meas
More Answers (0)
See Also
Categories
Find more on Execution Speed in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!
You can also select a web site from the following list
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom(English)
Asia Pacific
- Australia (English)
- India (English)
- New Zealand (English)
- 中国
- 日本Japanese (日本語)
- 한국Korean (한국어)