I'm Davide and I have a problem with the derivation of a function that later should be given to ode15i in Matlab.
Basically I've derived a big symbolic expression that describe the motion of a spececraft with a flexible appendice (like a solar panel). My goal is to obtain a function handle that can be integrated using the built-in implicit solver in Matlab (ode15i).
The problem I've encounter is the slowness of the Symbolic computations, especially in the function "daeFunction" (I've run it and lost any hope for a responce after 3/4 hours had passed).
The system of equations, that is derived using the Lagrange's method is an implicit ode.
The complex nature of the system arise from the flexibility modelling of the solar panel.
I am open to any suggestions that would help me in:
running the code properly.
running it as efficiently as possible.
Thx in advance.
Here after I copy the code. Note: Matlab r2021a was used.
close all
clear
clc
syms t
syms r(t) [3 1]
syms angle(t) [3 1]
syms delta(t)
syms beta(t) [3 1]
mu = 3.986e14;
mc = 1600;
mi = 10;
k = 10;
kt = 10;
Ii = [1 0 0 % for the first link it is different thus I should do a functoin or something that writes everything into an array or a vector
0 5 0
0 0 5];
% Dimension of satellite
a = 1;
b = 1.3;
c = 1;
Ic = 1/12*mc* [b^2+c^2 0 0
0 c^2+a^2 0
0 0 a^2+b^2];
ra_c = [0 1 0]';
a = diff(r,t,t);
ddelta = diff(delta,t);
dbeta = diff(beta,t);
dddelta = diff(delta,t,t);
ddbeta = diff(beta,t,t);
R= [cos(angle1).*cos(angle3)-cos(angle2).*sin(angle1).*sin(angle3) sin(angle1).*cos(angle3)+cos(angle2).*cos(angle1).*sin(angle3) sin(angle2).*sin(angle3)
-cos(angle1).*sin(angle3)-cos(angle2).*sin(angle1).*cos(angle3) -sin(angle1).*sin(angle3)+cos(angle2).*cos(angle1).*cos(angle3) sin(angle2).*cos(angle3)
sin(angle2).*sin(angle3) -sin(angle2).*cos(angle1) cos(angle2)];
d_angle1 = diff(angle1,t);
d_angle2 = diff(angle2,t);
d_angle3 = diff(angle3,t);
dd_angle1 = diff(angle1,t,t);
dd_angle2 = diff(angle2,t,t);
dd_angle3 = diff(angle3,t,t);
d_angle = [d_angle1;d_angle2;d_angle3];
dd_angle = [dd_angle1;dd_angle2;dd_angle3];
omega = [d_angle2.*cos(angle1)+d_angle3.*sin(angle2).*sin(angle1);d_angle2.*sin(angle1)-d_angle3.*sin(angle2).*cos(angle1);d_angle1+d_angle3.*cos(angle2)]; % this should describe correctly omega_oc
d_omega = diff(omega,t);
v1 = diff(r1,t);
v2 = diff(r2,t);
v3 = diff(r3,t);
v = [v1; v2; v3];
[J,r_cgi,R_ci]= Jacobian_Rob(4,delta,beta);
% Perform matrix multiplication
for mm = 1:4
vel(:,mm) = J(:,:,mm)*[ddelta;dbeta];
end
vel = formula(vel);
dr_Ccgi = vel(1:3,:);
omega_ci = vel(4:6,:);
assumeAlso(angle(t),'real');
assumeAlso(d_angle(t),'real');
assumeAlso(dd_angle(t),'real');
assumeAlso(r(t),'real');
assumeAlso(a(t),'real');
assumeAlso(v(t),'real');
assumeAlso(beta(t),'real');
assumeAlso(delta(t),'real');
assumeAlso(dbeta(t),'real');
assumeAlso(ddelta(t),'real');
assumeAlso(ddbeta(t),'real');
assumeAlso(dddelta(t),'real');
omega = formula(omega);
Tc = 1/2*v'*mc*v+1/2*omega'*R*Ic*R'*omega;
% kinetic energy of all appendices
for h = 1:4
Ti(h) = 1/2*v'*mi*v+mi*v'*skew(omega)*R*ra_c+mi*v'*skew(omega)*R*r_cgi(:,h)+mi*v'*R*dr_Ccgi(:,h)+1/2*mi*ra_c'*R'*skew(omega)'*skew(omega)*R*ra_c ...
+ mi*ra_c'*R'*skew(omega)'*skew(omega)*R*r_cgi(:,h)+mi*ra_c'*R'*skew(omega)'*R*dr_Ccgi(:,h)+1/2*omega'*R*R_ci(:,:,h)*Ii*(R*R_ci(:,:,h))'*omega ...
+ omega'*R*R_ci(:,:,h)*Ii*R_ci(:,:,h)'*omega_ci(:,h)+1/2*omega_ci(:,h)'*R_ci(:,:,h)*Ii*R_ci(:,:,h)'*omega_ci(:,h)+1/2*mi*r_cgi(:,h)'*R'*skew(omega)'*skew(omega)*R*r_cgi(:,h)+mi*r_cgi(:,h)'*R'*skew(omega)'*R*dr_Ccgi(:,h)...
+ 1/2*mi*dr_Ccgi(:,h)'*dr_Ccgi(:,h);
Ugi(h) = -mu*mi/norm(r,2)+mu*mi*r'/(norm(r,2)^3)*(R*ra_c+R*R_ci(:,:,h)*r_cgi(:,h));
end
Ugc = -mu*mc/norm(r,2);
Ue = 1/2*kt*(delta)^2+sum(1/2*k*(beta).^2);
U = Ugc+sum(Ugi)+Ue;
L = Tc + sum(Ti) - U;
D = 1/2 *100* (ddelta^2+sum(dbeta.^2));
%% Equation of motion derivation
eq = [diff(jacobian(L,v),t)'-jacobian(L,r)';
diff(jacobian(L,d_angle),t)'-jacobian(L,angle)';
diff(jacobian(L,ddelta),t)'-jacobian(L,delta)'+jacobian(D,ddelta)';
diff(jacobian(L,dbeta),t)'-jacobian(L,beta)'+jacobian(D,dbeta)'];
%% Reduction to first order sys
[sys,newVars,R1]=reduceDifferentialOrder(eq,[r(t); angle(t); delta(t); beta(t)]);
DAEs = sys;
DAEvars = newVars;
%% ode15i implicit solver
pDAEs = symvar(DAEs);
pDAEvars = symvar(DAEvars);
extraParams = setdiff(pDAEs,pDAEvars);
f = daeFunction(DAEs,DAEvars,'File','ProvaSum');
y0est = [6778e3 0 0 0.01 0.1 0.3 0 0.12 0 0 0 7400 0 0 0 0 0 0 0 0]';
yp0est = zeros(20,1);
opt = odeset('RelTol', 10.0^(-7),'AbsTol',10.0^(-7),'Stats', 'on');
[y0,yp0] = decic(f,0,y0est,[],yp0est,[],opt);
% Integration
[tSol,ySol] = ode15i(f,[0 0.5],y0,yp0,opt);
%% Funcitons
function [J,p_cgi,R_ci]=Jacobian_Rob(N,delta,beta)
% Function to compute Jacobian see Robotics by Siciliano
% N total number of links
% delta [1x1] beta [N-1x1] variable that describe position of the solar
% panel elements
beta = formula(beta);
L_link = [1 1 1 1]'; % Length of each link elements in [m], later to be derived from file or as function input
for I = 1 : N
A1 = Homog_Matrix(I,delta,beta);
A1 = formula(A1);
R_ci(:,:,I) = A1(1:3,1:3);
if I ~= 1
p_cgi(:,I) = A1(1:3,4) + A1(1:3,1:3)*[1 0 0]'*L_link(I)/2;
else
p_cgi(:,I) = A1(1:3,4) + A1(1:3,1:3)*[0 0 1]'*L_link(I)/2;
end
for j = 1:I
A_j = formula(Homog_Matrix(j,delta,beta));
z_j = A_j(1:3,3);
Jp(:,j) = skew(z_j)*(p_cgi(:,I)-A_j(1:3,4));
Jo(:,j) = z_j;
end
if N-I > 0
Jp(:,I+1:N) = zeros(3,N-I);
Jo(:,I+1:N) = zeros(3,N-I);
end
J(:,:,I)= [Jp;Jo];
end
J = formula(J);
p_cgi = formula(p_cgi);
R_ci = formula(R_ci);
end
function [A_CJ]=Homog_Matrix(J,delta,beta)
% This function is made sopecifically for the solar panel
% define basic rotation matrices
Rx = #(angle) [1 0 0
0 cos(angle) -sin(angle)
0 sin(angle) cos(angle)];
Ry = #(angle) [ cos(angle) 0 sin(angle)
0 1 0
-sin(angle) 0 cos(angle)];
Rz = #(angle) [cos(angle) -sin(angle) 0
sin(angle) cos(angle) 0
0 0 1];
if isa(beta,"sym")
beta = formula(beta);
end
L_link = [1 1 1 1]'; % Length of each link elements in [m], later to be derived from file or as function input
% Rotation matrix how C sees B
R_CB = Rz(-pi/2)*Ry(-pi/2); % Clarify notation: R_CB represent the rotation matrix that describe the frame B how it is seen by C
% it is the same if it was wrtitten R_B2C
% becouse bring a vector written in B to C
% frame --> p_C = R_CB p_B
% same convention used in siciliano how C sees B frame
A_AB = [R_CB zeros(3,1)
zeros(1,3) 1];
A_B1 = [Rz(delta) zeros(3,1)
zeros(1,3) 1];
A_12 = [Ry(-pi/2)*Rx(-pi/2)*Rz(beta(1)) L_link(1)*[0 0 1]'
zeros(1,3) 1];
if J == 1
A_CJ = A_AB*A_B1;
elseif J == 0
A_CJ = A_AB;
else
A_CJ = A_AB*A_B1*A_12;
end
for j = 3:J
A_Jm1J = [Rz(beta(j-1)) L_link(j-1)*[1 0 0]'
zeros(1,3) 1];
A_CJ = A_CJ*A_Jm1J;
end
end
function [S]=skew(r)
S = [ 0 -r(3) r(2); r(3) 0 -r(1); -r(2) r(1) 0];
end
I found your question beautiful. My suggestion is to manipulate the problem numerically. symbolic manipulation in Matlab is good but is much slower than numerical calculation. you can define easily the ode into a system of first-order odes and solve them using numerical integration functions like ode45. Your code is very lengthy and I couldn't manage to follow its details.
All the Best.
Yasien
I have some matlab code as follow, constructing KNN similarity weight matrix.
[D,I] = pdist2(X, X, 'squaredeuclidean', 'Smallest', k+1);
D = D < threshold;
W = zeros(n, n);
for i=1:size(I,2)
W(I(:,i), i) = D(:,i);
W(i, I(:,i)) = D(:,i)';
end
I want to vectorize the for loop. I have tried
W(I) = D;
but failed to get the correct value.
I add test case here:
n = 5;
D = [
1 1 1 1 1
0 1 1 1 1
0 0 0 0 0
];
I = [
1 2 3 4 5
5 4 5 2 3
3 1 1 1 1
];
There are some undefined variables that makes it hard to check what it is doing, but this should do the same as your for loop:
D,I] = pdist2(X, X, 'squaredeuclidean', 'Smallest', k+1);
D = D < threshold;
W = zeros(n);
% set the diagonal values
W(sub2ind(size(X), I(1, :), I(1, :))) = D(1,:);
% set the other values
W(sub2ind(size(W), I(2, :), 1:size(I, 2))) = D(2, :);
W(sub2ind(size(W), 1:size(I, 2), I(2, :))) = D(2, :).';
I splited the directions, it works now with your test case.
A possible solution:
idx1 = reshape(1:n*n,n,n).';
idx2 = bsxfun(#plus,I,0:n:n*size(I,2)-1);
W=zeros(n,n);
W(idx2) = D;
W(idx1(idx2)) = D;
Here assumed that you repeatedly want to compute D and I so compute idx only one time and use it repeatedly.
n = 5;
idx1 = reshape(1:n*n,n,n).';
%for k = 1 : 1000
%[D,I] = pdist2(X, X, 'squaredeuclidean', 'Smallest', k+1);
%D = D < threshold;
idx2 = bsxfun(#plus,I,0:n:n*size(I,2)-1);
W=zeros(n,n);
W(idx2) = D;
W(idx1(idx2)) = D;
%end
But if n isn't constant and it varies in each iteration it is better to change the way idx1 is computed:
n = 5;
%for k = 1 : 1000
%n = randi([2 10]);%n isn't constant
%[D,I] = pdist2(X, X, 'squaredeuclidean', 'Smallest', k+1);
%D = D < threshold;
idx1 = bsxfun(#plus,(0:n:n^2-1).',1:size(I,2));
idx2 = bsxfun(#plus,I,0:n:n*size(I,2)-1);
W=zeros(n,n);
W(idx2) = D;
W(idx1(idx2)) = D;
%end
You can cut some corners with linear indices but if your matrices are big then you should only take the nonzero components of D. Following copies all values of D
W = zeros(n);
W(reshape(sub2ind([n,n],I,[1;1;1]*[1:n]),1,[])) = reshape(D,1,[]);
I saw other topics about this error but I couldn't figure it out. The error "In an assignment A(I) = B, the number of elements in B and I must be the same" occurs at the second for loop. How can I change my code to avoid this error?
h1 = [70 31.859 15 5.774 3.199 2.15 1.626];
h2 = [31.859 15 5.774 3.199 2.15 1.626 1.415];
b = [1253 1253 1253 1253 1253 1253 1253];
R = [455.4 425.6 377.6 374.9 371.3 273.7 268.3];
r = [0.5448714286 0.5291754292 0.6150666667 0.4459646692 0.3279149734 0.2437209302 0.1297662977];
k = [200 200 200 200 200 200 200];
s = sqrt(r/(1-r));
v2 = [20 0 0 0 0 0 0];
v1 = [0 0 0 0 0 0 0];
Ch1 = [0 0 0 0 0 0 0];
Ch2 = [0 0 0 0 0 0 0];
C = [100 100 100 100 100 100 100];
F = b .* k .* sqrt(R-(h1-h2))- R.*sin((acos((R-((h1-h2)./2))./R))) .* (pi/2) .* (1./sqrt(r./(1-r))) .* (atan(sqrt(r./(1-r))))-(pi/4) - (1./(sqrt(r./(1-r)) .* sqrt(h2./R))).* log((h2+R.*((sqrt(h1./R).*tan(1/2 .* atan(sqrt(r./(1-r)).*sqrt(h1./r).*log(1./(1-k))))).^2).*sqrt(1-r))./h2)
M = -R.*R.*(k./2).*(.2*(sqrt(h2./R)*tan(0.5*(atan(s)))-(pi/8).*sqrt(h2./R).*log(1./1-r)))-(acos((R-((h1-h2)./2))./R))
for i=1:6
v1(i) = ((v2(i)*h2)/h1);
v2(i+1) = v1(i);
end
vr = ((v1.*h1)./h2)./(((tan(0.5.*((atan(s)))-(pi/8).*sqrt(h2./R).*log(1./(1-r)))).^(2))+1)
%--------------------------------------------------------------------------
% Calculating E
w = (((2.*R.*h2).^(3/2))./(300.*(b.^2)))
if (w <= (3*10^-4));
E = ((0.0821.*((log(w))^2))+(1.25.*log(w))+4.89)
end
if ((3*10^-4)<= w <= (2.27*10^-3));
E = ((0.0172.*((log(w)).^2))+(0.175.*log(w))+0.438)
end
if (w > (2.27*10^-3))
E = 0.01
end
%--------------------------------------------------------------------------
% Calculating Ch:
y = ((((2.*R).^(0.5)).*((h2).^(1.5)))./(b.^2))
N1 = (0.5-(1/pi).*atan((log(y)+8.1938)./(1.1044)))
N = ((h2./h1).*N1)
for i=1:1;7
Ch2(i) = (h2.*((N.*((Ch1(i)./h2)-(C./h2)))+(C/h2)))
Ch1(i+1) = Ch2(i)
end
DeltaStrain = (E.*((Ch2./h2)-(Ch1./h1)))
if DeltaStrain > 0;
Stepp = ((2/pi).*(sqrt(DeltaStrain))))
Control = 2;
else
Stepp = ((2/pi).*(sqrt(-DeltaStrain))
Control = 0;
end
In the line
Ch2(i) = (h2.*((N.*((Ch1(i)./h2)-(C./h2)))+(C/h2)))
h2 is a vector, and Ch2(i) is a scalar. You cannot assign the value of a vector to a scalar. I suspect you want to replace the entire for loop. Right now you have
for i=1:1;7
Ch2(i) = (h2.*((N.*((Ch1(i)./h2)-(C./h2)))+(C/h2)))
Ch1(i+1) = Ch2(i)
end
(?? what is the meaning of 1:1;7? Is that a typo? I am thinking you want 1:7...
Since you seem to be using the result of one loop to change the value of Ch1 which you are using again in the next loop, it may be tricky to vectorize; but I wonder what you are expecting the output to be, since you really do have a vector as the result of the RHS of the equation. I can't be sure if you want to compute the result for one element at a time, or whether you want to compute vectors (and end up appending results to Ch1 and Ch2). The following line would run without throwing an error - but it may not be the calculation you want. Please clarify what you are hoping to achieve if this is an incorrect guess.
for i = 1:7
Ch2(i) = h2.*(N.*((Ch1(i) - C(i))./h2(i))) + C(i)./h2(i);
Ch1(i+1) = Ch2(i);
end
I am trying to generate a pn sequence and it works. However, when I try I call the function with different inputs in a for-loop, it gives me the same results each time. As if it is not affected by using the for loop. Why?
This is my code:
%e.g. noof flip flops 4 ==>
function[op_seq]=pnseq(a,b,c)
a = 7;
%generator polynomial x4+x+1 ==>
b = [1 0 0 1 1 0 1 ]
%initial state [1 0 0 0] ==>
c = [1 0 0 0 1 0 1 ]
%refere figure to set a relation between tap function and initial state
%
for j= 1:50,
x = a;
tap_ff =b;
int_stat= c;
for i = 1:1: length(int_stat)
old_stat(i) = int_stat(i);
gen_pol(i) = tap_ff(i);
end
len = (2 ^x)-1;
gen_pol(i+1)= 1;
gen_l = length(gen_pol);
old_l = length(old_stat);
for i1 = 1: 1:len
% feed back input genration
t = 1;
for i2 = 1:old_l
if gen_pol(i2)==1
stat_str(t) = old_stat(gen_l - i2);
i2 = i2+1;
t = t+1;
else
i2 = i2+1;
end
end
stat_l = length(stat_str);
feed_ip = stat_str(1);
for i3 = 1: stat_l-1
feed_ip = bitxor(feed_ip,stat_str(i3 + 1));
feed_ipmag(i1) = feed_ip;
i3 = i3+1;
end
% shifting elements
new_stat = feed_ip;
for i4 = 1:1:old_l
new_stat(i4+1) = old_stat(i4);
old_stat(i4)= new_stat(i4);
end
op_seq(i1) = new_stat(old_l +1);
end
%op_seq;
end
I assume you're doing something like:
for n = 1:10
...
% set a,b,c for this n
...
op_seq =pnseq(a,b,c)
...
end
and that you see the same op_seq output for each case. This is because you have a,b,c as inputs, but you overwrite them at the start of your function. If I remove, or comment out the following lines in your function:
a = 7;
b = [1 0 0 1 1 0 1 ]
c = [1 0 0 0 1 0 1 ]
Then I get different results for calling the function with different a,b,c. There is nothing random in your function, so the same inputs give the same outputs.