Related
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 need to create a binary mask. I have some coordinates and I make those coordinates and inside that region equal to 1, and background equal to zero.
Here is what I have done, but the problem is ROI located not in the correct position and located on the right bottom of the image. I appreciate if someone could point me to the right direction.
function [X,Y, BW] = Create_mask(X,Y,im)
X = round(X);
Y = round(Y);
X ( X < 1 ) = 1;
Y ( Y < 1 ) = 1;
BW = im > 255;
for p = 1:length(X)
BW(Y(p),X(p)) = 1;
end
for n = 0:(1/round(sqrt((X(end)-X(1))^2 + (Y(end)-Y(1))^2 ))):1
xn = round(X(1) +(X(end) - X(1))*n);
yn = round(Y(1) +(Y(end) - Y(1))*n);
BW(yn,xn) = 1;
end
se = strel('disk',10);
BW = imclose(BW,se);
BW = imdilate(BW,se);
BW = imfill(BW,'holes');
im( im < 255 ) = 0;
im = imclose(im,se);
BW = BW * 255;
BW = im2uint8(BW);
% BW = imresize(BW, [256 256],'nearest');
figure;
imshow(BW);
% close all;
end
Here is the output the function:
I was expecting to be similar to this image. This is not the exact solution but it shows my expectation.
X and Y coordinates are attached here, The first col is X and the second Y.
you can do this by calling function inpolygon, try this
function mask=createmask(x,y, cmin, cmax, dx)
if(nargin<3)
cmin=min([x(:) y(:)]);
end
if(nargin<4)
cmax=max([x(:) y(:)]);
end
if(nargin<5)
dx=(cmax-cmin)/100;
end
if(length(dx)==1)
dx=[dx dx];
end
[xi,yi]=meshgrid(cmin(1):dx(1):cmax(1),cmin(2):dx(2):cmax(2));
mask=reshape(inpolygon(xi(:),yi(:),x(:),y(:)), size(xi));
to test
xv = [0 3 3 0 0 NaN 1 1 2 2 1];
yv = [0 0 3 3 0 NaN 1 2 2 1 1];
mask=createmask(xv,yv, [-1 -1], [4 4]);
imagesc(mask)
I want to speed up my code. I always use vectorization. But in this code I have no idea how to avoid the for-loop. I would really appreciate a hint how to proceed.
thank u so much for your time.
close all
clear
clc
% generating sample data
x = linspace(10,130,33);
y = linspace(20,100,22);
[xx, yy] = ndgrid(x,y);
k = 2*pi/50;
s = [sin(k*xx+k*yy)];
% generating query points
xi = 10:5:130;
yi = 20:5:100;
[xxi, yyi] = ndgrid(xi,yi);
P = [xxi(:), yyi(:)];
% interpolation algorithm
dx = x(2) - x(1);
dy = y(2) - y(1);
x_ = [x(1)-dx x x(end)+dx x(end)+2*dx];
y_ = [y(1)-dy y y(end)+dy y(end)+2*dy];
s_ = [s(1) s(1,:) s(1,end) s(1,end)
s(:,1) s s(:,end) s(:,end)
s(end,1) s(end,:) s(end,end) s(end,end)
s(end,1) s(end,:) s(end,end) s(end,end)];
si = P(:,1)*0;
M = 1/6*[-1 3 -3 1
3 -6 3 0
-3 0 3 0
1 4 1 0];
tic
for nn = 1:numel(P(:,1))
u = mod(P(nn,1)- x_(1), dx)/dx;
jj = floor((P(nn,1) - x_(1))/dx) + 1;
v = mod(P(nn,2)- y_(1), dy)/dy;
ii = floor((P(nn,2) - y_(1))/dy) + 1;
D = [s_(jj-1,ii-1) s_(jj-1,ii) s_(jj-1,ii+1) s_(jj-1,ii+2)
s_(jj,ii-1) s_(jj,ii) s_(jj,ii+1) s_(jj,ii+2)
s_(jj+1,ii-1) s_(jj+1,ii) s_(jj+1,ii+1) s_(jj+1,ii+2)
s_(jj+2,ii-1) s_(jj+2,ii) s_(jj+2,ii+1) s_(jj+2,ii+2)];
U = [u.^3 u.^2 u 1];
V = [v.^3 v.^2 v 1];
si(nn) = U*M*D*M'*V';
end
toc
scatter3(P(:,1), P(:,2), si)
hold on
mesh(xx,yy,s)
This is the full example and is a cubic B-spline surface interpolation algorithm in 2D space.
I have a 3D volume and a 2D image and an approximate mapping (affine transformation with no skwewing, known scaling, rotation and translation approximately known and need fitting) between the two. Because there is an error in this mapping and I would like to further register the 2D image to the 3D volume. I have not written code for registration purposes before, but because I can't find any programs or code to solve this I would like to try and do this. I believe the standard for registration is to optimize mutual information. I think this would also be suitable here, because the intensities are not equal between the two images. So I think I should make a function for the transformation, a function for the mutual information and a function for optimization.
I did find some Matlab code on a mathworks thread from two years ago, based on an article. The OP reports that she managed to get the code to work, but I'm not getting how she did that exactly. Also in the IP package for matlab there is an implementation, but I dont have that package and there does not seem to be an equivalent for octave. SPM is a program that uses matlab and has registration implemented, but does not cope with 2d to 3d registration. On the file exchange there is a brute force method that registers two 2D images using mutual information.
What she does is pass a multi planar reconstruction function and an similarity/error function into a minimization algorithm. But the details I don't quite understand. Maybe it would be better to start fresh:
load mri; volume = squeeze(D);
phi = 3; theta = 2; psi = 5; %some small angles
tx = 1; ty = 1; tz = 1; % some small translation
dx = 0.25, dy = 0.25, dz = 2; %different scales
t = [tx; ty; tz];
r = [phi, theta, psi]; r = r*(pi/180);
dims = size(volume);
p0 = [round(dims(1)/2);round(dims(2)/2);round(dims(3)/2)]; %image center
S = eye(4); S(1,1) = dx; S(2,2) = dy; S(3,3) = dz;
Rx=[1 0 0 0;
0 cos(r(1)) sin(r(1)) 0;
0 -sin(r(1)) cos(r(1)) 0;
0 0 0 1];
Ry=[cos(r(2)) 0 -sin(r(2)) 0;
0 1 0 0;
sin(r(2)) 0 cos(r(2)) 0;
0 0 0 1];
Rz=[cos(r(3)) sin(r(3)) 0 0;
-sin(r(3)) cos(r(3)) 0 0;
0 0 1 0;
0 0 0 1];
R = S*Rz*Ry*Rx;
%make affine matrix to rotate about center of image
T1 = ( eye(3)-R(1:3,1:3) ) * p0(1:3);
T = T1 + t; %add translation
A = R;
A(1:3,4) = T;
Rold2new = A;
Rnew2old = inv(Rold2new);
%the transformation
[xx yy zz] = meshgrid(1:dims(1),1:dims(2),1:1);
coordinates_axes_new = [xx(:)';yy(:)';zz(:)'; ones(size(zz(:)))'];
coordinates_axes_old = Rnew2old*coordinates_axes_new;
Xcoordinates = reshape(coordinates_axes_old(1,:), dims(1), dims(2), dims(3));
Ycoordinates = reshape(coordinates_axes_old(2,:), dims(1), dims(2), dims(3));
Zcoordinates = reshape(coordinates_axes_old(3,:), dims(1), dims(2), dims(3));
%interpolation/reslicing
method = 'cubic';
slice= interp3(volume, Xcoordinates, Ycoordinates, Zcoordinates, method);
%so now I have my slice for which I would like to find the correct position
% first guess for A
A0 = eye(4); A0(1:3,4) = T1; A0(1,1) = dx; A0(2,2) = dy; A0(3,3) = dz;
% this is pretty close to A
% now how would I fit the slice to the volume by changing A0 and examining some similarity measure?
% probably maximize mutual information?
% http://www.mathworks.com/matlabcentral/fileexchange/14888-mutual-information-computation/content//mi/mutualinfo.m
Ok I was hoping for someone else's approach, that would probably have been better than mine as I have never done any optimization or registration before. So I waited for Knedlsepps bounty to almost finish. But I do have some code thats working now. It will find a local optimum so the initial guess must be good. I wrote some functions myself, took some functions from the file exchange as is and I extensively edited some other functions from the file exchange. Now that I put all the code together to work as an example here, the rotations are off, will try and correct that. Im not sure where the difference in code is between the example and my original code, must have made a typo in replacing some variables and data loading scheme.
What I do is I take the starting affine transformation matrix, decompose it to an orthogonal matrix and an upper triangular matrix. I then assume the orthogonal matrix is my rotation matrix so I calculate the euler angles from that. I directly take the translation from the affine matrix and as stated in the problem I assume I know the scaling matrix and there is no shearing. So then I have all degrees of freedom for the affine transformation, which my optimisation function changes and constructs a new affine matrix from, applies it to the volume and calculates the mutual information. The matlab optimisation function patternsearch then minimises 1-MI/MI_max.
What I noticed when using it on my real data which are multimodal brain images is that it works much better on brain extracted images, so with the skull and tissue outside of the skull removed.
%data
load mri; volume = double(squeeze(D));
%transformation parameters
phi = 3; theta = 1; psi = 5; %some small angles
tx = 1; ty = 1; tz = 3; % some small translation
dx = 0.25; dy = 0.25; dz = 2; %different scales
t = [tx; ty; tz];
r = [phi, theta, psi]; r = r*(pi/180);
%image center and size
dims = size(volume);
p0 = [round(dims(1)/2);round(dims(2)/2);round(dims(3)/2)];
%slice coordinate ranges
range_x = 1:dims(1)/dx;
range_y = 1:dims(2)/dy;
range_z = 1;
%rotation
R = dofaffine([0;0;0], r, [1,1,1]);
T1 = ( eye(3)-R(1:3,1:3) ) * p0(1:3); %rotate about p0
%scaling
S = eye(4); S(1,1) = dx; S(2,2) = dy; S(3,3) = dz;
%translation
T = [[eye(3), T1 + t]; [0 0 0 1]];
%affine
A = T*R*S;
% first guess for A
r00 = [1,1,1]*pi/180;
R00 = dofaffine([0;0;0], r00, [1 1 1]);
t00 = T1 + t + ( eye(3) - R00(1:3,1:3) ) * p0;
A0 = dofaffine( t00, r00, [dx, dy, dz] );
[ t0, r0, s0 ] = dofaffine( A0 );
x0 = [ t0.', r0, s0 ];
%the transformation
slice = affine3d(volume, A, range_x, range_y, range_z, 'cubic');
guess = affine3d(volume, A0, range_x, range_y, range_z, 'cubic');
%initialisation
Dt = [1; 1; 1];
Dr = [2 2 2].*pi/180;
Ds = [0 0 0];
Dx = [Dt', Dr, Ds];
%limits
LB = x0-Dx;
UB = x0+Dx;
%other inputs
ref_levels = length(unique(slice));
Qref = imquantize(slice, ref_levels);
MI_max = MI_GG(Qref, Qref);
%patternsearch options
options = psoptimset('InitialMeshSize',0.03,'MaxIter',20,'TolCon',1e-5,'TolMesh',5e-5,'TolX',1e-6,'PlotFcns',{#psplotbestf,#psplotbestx});
%optimise
[x2, MI_norm_neg, exitflag_len] = patternsearch(#(x) AffRegOptFunc(x, slice, volume, MI_max, x0), x0,[],[],[],[],LB(:),UB(:),options);
%check
p0 = [round(size(volume)/2).'];
R0 = dofaffine([0;0;0], x2(4:6)-x0(4:6), [1 1 1]);
t1 = ( eye(3) - R0(1:3,1:3) ) * p0;
A2 = dofaffine( x2(1:3).'+t1, x2(4:6), x2(7:9) ) ;
fitted = affine3d(volume, A2, range_x, range_y, range_z, 'cubic');
overlay1 = imfuse(slice, guess);
overlay2 = imfuse(slice, fitted);
figure(101);
ax(1) = subplot(1,2,1); imshow(overlay1, []); title('pre-reg')
ax(2) = subplot(1,2,2); imshow(overlay2, []); title('post-reg');
linkaxes(ax);
function normed_score = AffRegOptFunc( x, ref_im, reg_im, MI_max, x0 )
t = x(1:3).';
r = x(4:6);
s = x(7:9);
rangx = 1:size(ref_im,1);
rangy = 1:size(ref_im,2);
rangz = 1:size(ref_im,3);
ref_levels = length(unique(ref_im));
reg_levels = length(unique(reg_im));
t0 = x0(1:3).';
r0 = x0(4:6);
s0 = x0(7:9);
p0 = [round(size(reg_im)/2).'];
R = dofaffine([0;0;0], r-r0, [1 1 1]);
t1 = ( eye(3) - R(1:3,1:3) ) * p0;
t = t + t1;
Ap = dofaffine( t, r, s );
reg_im_t = affine3d(reg_im, A, rangx, rangy, rangz, 'cubic');
Qref = imquantize(ref_im, ref_levels);
Qreg = imquantize(reg_im_t, reg_levels);
MI = MI_GG(Qref, Qreg);
normed_score = 1-MI/MI_max;
end
function [ varargout ] = dofaffine( varargin )
% [ t, r, s ] = dofaffine( A )
% [ A ] = dofaffine( t, r, s )
if nargin == 1
%affine to degrees of freedom (no shear)
A = varargin{1};
[T, R, S] = decompaffine(A);
r = GetEulerAngles(R(1:3,1:3));
s = [S(1,1), S(2,2), S(3,3)];
t = T(1:3,4);
varargout{1} = t;
varargout{2} = r;
varargout{3} = s;
elseif nargin == 3
%degrees of freedom to affine (no shear)
t = varargin{1};
r = varargin{2};
s = varargin{3};
R = GetEulerAngles(r); R(4,4) = 1;
S(1,1) = s(1); S(2,2) = s(2); S(3,3) = s(3); S(4,4) = 1;
T = eye(4); T(1,4) = t(1); T(2,4) = t(2); T(3,4) = t(3);
A = T*R*S;
varargout{1} = A;
else
error('incorrect number of input arguments');
end
end
function [ T, R, S ] = decompaffine( A )
%I assume A = T * R * S
T = eye(4);
R = eye(4);
S = eye(4);
%decompose in orthogonal matrix q and upper triangular matrix r
%I assume q is a rotation matrix and r is a scale and shear matrix
%matlab 2014 can force real solution
[q r] = qr(A(1:3,1:3));
R(1:3,1:3) = q;
S(1:3,1:3) = r;
% A*S^-1*R^-1 = T*R*S*S^-1*R^-1 = T*R*I*R^-1 = T*R*R^-1 = T*I = T
T = A*inv(S)*inv(R);
t = T(1:3,4);
T = [eye(4) + [[0 0 0;0 0 0;0 0 0;0 0 0],[t;0]]];
end
function [varargout]= GetEulerAngles(R)
assert(length(R)==3)
dims = size(R);
if min(dims)==1
rx = R(1); ry = R(2); rz = R(3);
R = [[ cos(ry)*cos(rz), -cos(ry)*sin(rz), sin(ry)];...
[ cos(rx)*sin(rz) + cos(rz)*sin(rx)*sin(ry), cos(rx)*cos(rz) - sin(rx)*sin(ry)*sin(rz), -cos(ry)*sin(rx)];...
[ sin(rx)*sin(rz) - cos(rx)*cos(rz)*sin(ry), cos(rz)*sin(rx) + cos(rx)*sin(ry)*sin(rz), cos(rx)*cos(ry)]];
varargout{1} = R;
else
ry=asin(R(1,3));
rz=acos(R(1,1)/cos(ry));
rx=acos(R(3,3)/cos(ry));
if nargout > 1 && nargout < 4
varargout{1} = rx;
varargout{2} = ry;
varargout{3} = rz;
elseif nargout == 1
varargout{1} = [rx ry rz];
else
error('wrong number of output arguments');
end
end
end
I'm doing a homework assignment for scientific computing, specifically the iterative methods Gauss-Seidel and SOR in matlab, the problem is that for a matrix gives me unexpected results (the solution does not converge) and for another matrix converges.
Heres the code of sor, where:
A: Matrix of the system A * x = b
Xini: array of initial iteration
b: array independent of the system A * x = b
maxiter: Maximum Iterations
tol: Tolerance;
In particular, the SOR method, will receive a sixth parameter called w which corresponds to the relaxation parameter.
Here´s the code for sor method:
function [x2,iter] = sor(A,xIni, b, maxIter, tol,w)
x1 = xIni;
x2 = x1;
iter = 0;
i = 0;
j = 0;
n = size(A, 1);
for iter = 1:maxIter,
for i = 1:n
a = w / A(i,i);
x = 0;
for j = 1:i-1
x = x + (A(i,j) * x2(j));
end
for j = i+1:n
x = x + (A(i,j) * x1(j));
end
x2(i) = (a * (b(i) - x)) + ((1 - w) * x1(i));
end
x1 = x2;
if (norm(b - A * x2) < tol);
break;
end
end
Here´s the code for Gauss-seidel method:
function [x, iter] = Gauss(A, xIni, b, maxIter, tol)
x = xIni;
xnew = x;
iter = 0;
i = 0;
j = 0;
n = size(A,1);
for iter = 1:maxIter,
for i = 1:n
a = 1 / A(i,i);
x1 = 0;
x2 = 0;
for j = 1:i-1
x1 = x1 + (A(i,j) * xnew(j));
end
for j = i+1:n
x2 = x2 + (A(i,j) * x(j));
end
xnew(i) = a * (b(i) - x1 - x2);
end
x= xnew;
if ((norm(A*xnew-b)) <= tol);
break;
end
end
For this input:
A = [1 2 -2; 1 1 1; 2 2 1];
b = [1; 2; 5];
when call the function Gauss-Seidel or sor :
[x, iter] = gauss(A, [0; 0; 0], b, 1000, eps)
[x, iter] = sor(A, [0; 0; 0], b, 1000, eps, 1.5)
the output for gauss is:
x =
1.0e+304 *
1.6024
-1.6030
0.0011
iter =
1000
and for sor is:
x =
NaN
NaN
NaN
iter =
1000
however for the following system is able to find the solution:
A = [ 4 -1 0 -1 0 0;
-1 4 -1 0 -1 0;
0 -1 4 0 0 -1;
-1 0 0 4 -1 0;
0 -1 0 -1 4 -1;
0 0 -1 0 -1 4 ]
b = [1 0 0 0 0 0]'
Solution:
[x, iter] = sor(A, [0; 0; 0], b, 1000, eps, 1.5)
x =
0.2948
0.0932
0.0282
0.0861
0.0497
0.0195
iter =
52
The behavior of the methods depends on the conditioning of both matrices? because I noticed that the second matrix is better conditioned than the first. Any suggestions?
From the wiki article on Gauss-Seidel:
convergence is only guaranteed if the matrix is either diagonally dominant, or symmetric and positive definite
Since SOR is similar to Gauss-Seidel, I expect the same conditions to hold for SOR, but you might want to look that one up.
Your first matrix is definitely not diagonally dominant or symmetric. Your second matrix however, is symmetric and positive definite (because all(A==A.') and all(eig(A)>0)).
If you use Matlab's default method (A\b) as the "real" solution, and you plot the norm of the difference between each iteration and the "real" solution, then you get the two graphs below. It is obvious the first matrix is not ever going to converge, while the second matrix already produces acceptable results after a few iterations.
Always get to know the limitations of your algorithms before applying them in the wild :)