My version of MATLAB doesn't have the quatrotate function included, so I wrote my own using the equation MathWorks provide here. Trouble is, I don't get the same answers they get in their example in my function, or when I hand calculate it.
Under their example if I input the following I should get an n vector [-1 1 1]:
q = [1 0 1 0]; r = [1 1 1]; n = quatrotate(q, r)
n =
-1.0000 1.0000 1.0000
In my function, and by hand, I get:
[-3 1 1]
What am I missing here? The more I search the more confused I get. As far as I can tell the answer should be [-3 1 1].
Here is the function I wrote:
function [n] = quatrotate(q,r)
%Rotate a given acceleration vector by a given quaternion
%
%Inputs:
% q: A matrix containing a set of quaternion vectors of the
% form q = [w,x,y,z]
% r: A matrix containing a set of linear acceleration vectors
% of the form r= [i,j,k] (also known as [x,y,z])
%
% Outputs:
% n: The solved matrix containing the rotated vector of each linear
% acceleration component
%
%This assumes that the quaternion is normalised (sqw + sqx + sqy + sqz =1),
%if not it should be normalised before doing the conversion.
%To normalise divide qx, qy, qz and qw by n where n=sqrt(qx2 + qy2 + qz2 + qw2)
for k = 1:size(q,1)
rot=[(1-2.*q(k,3).^2-2.*q(k,4).^2) 2.*(q(k,2).*q(k,3)+q(k,1).*q(k,4))...
2.*(q(k,2).*q(k,4)-q(k,1).*q(k,3));2.*(q(k,2).*q(k,3)-q(k,1).*q(k,4))...
(1-2.*q(k,2).^2-2.*q(k,4).^2) 2.*(q(k,3).*q(k,4)+q(k,1).*q(k,2));...
2.*(q(k,2).*q(k,4)+q(k,1).*q(k,3)) 2.*(q(k,3).*q(k,4)-q(k,1).*q(k,2))...
(1-2.*q(k,2).^2-2.*q(k,3).^2)];
n(k,:) = rot*r(k,:)';
end
Thanks in advance!
first of all you need to calculate the modulus of the given Quaternion q:
for index = size(q,1):-1:1
mod(index,:) = norm(q(index,:),2);
end
Then normalize it:
qn = q./(mod* ones(1,4));
Now calculate the Direct Cosine Matrix using these formulae:
dcm = zeros(3,3,size(qn,1));
dcm(1,1,:) = qn(:,1).^2 + qn(:,2).^2 - qn(:,3).^2 - qn(:,4).^2;
dcm(1,2,:) = 2.*(qn(:,2).*qn(:,3) + qn(:,1).*qn(:,4));
dcm(1,3,:) = 2.*(qn(:,2).*qn(:,4) - qn(:,1).*qn(:,3));
dcm(2,1,:) = 2.*(qn(:,2).*qn(:,3) - qn(:,1).*qn(:,4));
dcm(2,2,:) = qn(:,1).^2 - qn(:,2).^2 + qn(:,3).^2 - qn(:,4).^2;
dcm(2,3,:) = 2.*(qn(:,3).*qn(:,4) + qn(:,1).*qn(:,2));
dcm(3,1,:) = 2.*(qn(:,2).*qn(:,4) + qn(:,1).*qn(:,3));
dcm(3,2,:) = 2.*(qn(:,3).*qn(:,4) - qn(:,1).*qn(:,2));
dcm(3,3,:) = qn(:,1).^2 - qn(:,2).^2 - qn(:,3).^2 + qn(:,4).^2;
According to MATLAB documents, the rotation of a vector r by the calculated dcm can be found as follows:
if ( size(q,1) == 1 )
% Q is 1-by-4
qout = (dcm*r')';
elseif (size(r,1) == 1)
% R is 1-by-3
for i = size(q,1):-1:1
qout(i,:) = (dcm(:,:,i)*r')';
end
else
% Q is M-by-4 and R is M-by-3
for i = size(q,1):-1:1
qout(i,:) = (dcm(:,:,i)*r(i,:)')';
end
end
Well first of all, in order for quatrotate to work, you need to use a unit quaternion (i.e. length of 1).
Second of all, I see that you are using the matrix provided by the MATLAB page. I've recently derived that matrix myself and found that the MATLAB page has the wrong matrix.
According to this (page 45), rotating a vector by a quaternion is
p' = p + 2w(v × p)+2(v × (v × p))
Where,
p' is the output vector after rotation,
p is the starting vector to be rotated,
w is the first coefficient in the quaternion,
v is the vector of [second coefficent, third coefficient, fourth coefficient] of the quaternion,
× is the cross product operand.
I encourage you to go to the link and derive the matrix yourself. You will see that the matrix provided on the MATLAB page has the wrong additions and subtractions.
This is what's said on the MATLAB page:
Here's my derivation (here the quaternion is [q0, q1, q2, q3] and the vector is [x, y, z]):
First row:
Second row:
Third row:
Here you can see that the signs are incorrect on MATLAB's website. I've emailed them about the error and is waiting to hear back.
Related
I'm trying to run a program in matlab to obtain the direct and inverse DFT for a grey scale image, but I'm not able to recover the original image after applying the inverse. I'm getting complex numbers as my inverse output. Is like i'm losing information. Any ideas on this? Here is my code:
%2D discrete Fourier transform
%Image Dimension
M=3;
N=3;
f=zeros(M,N);
f(2,1:3)=1;
f(3,1:3)=0.5;
f(1,2)=0.5;
f(3,2)=1;
f(2,2)=0;
figure;imshow(f,[0 1],'InitialMagnification','fit')
%Direct transform
for u=0:1:M-1
for v=0:1:N-1
for x=1:1:M
for y=1:1:N
F(u+1,v+1)=f(x,y)*exp(-2*pi*(1i)*((u*(x-1)/M)+(v*(y-1)/N)));
end
end
end
end
Fab=abs(F);
figure;imshow(Fab,[0 1],'InitialMagnification','fit')
%Inverse Transform
for x=0:1:M-1
for y=0:1:N-1
for u=1:1:M
for v=1:1:N
z(x+1,y+1)=(1/M*N)*F(u,v)*exp(2*pi*(1i)*(((u-1)*x/M)+((v-1)*y/N)));
end
end
end
end
figure;imshow(real(z),[0 1],'InitialMagnification','fit')
There are a couple of issues with your code:
You are not applying the definition of the DFT (or IDFT) correctly: you need to sum over the original variable(s) to obtain the transform. See the formula here; notice the sum.
In the IDFT the normalization constant should be 1/(M*N) (not 1/M*N).
Note also that the code could be made mucho more compact by vectorization, avoiding the loops; or just using the fft2 and ifft2 functions. I assume you want to compute it manually and "low-level" to verify the results.
The code, with the two corrections, is as follows. The modifications are marked with comments.
M=3;
N=3;
f=zeros(M,N);
f(2,1:3)=1;
f(3,1:3)=0.5;
f(1,2)=0.5;
f(3,2)=1;
f(2,2)=0;
figure;imshow(f,[0 1],'InitialMagnification','fit')
%Direct transform
F = zeros(M,N); % initiallize to 0
for u=0:1:M-1
for v=0:1:N-1
for x=1:1:M
for y=1:1:N
F(u+1,v+1) = F(u+1,v+1) + ...
f(x,y)*exp(-2*pi*(1i)*((u*(x-1)/M)+(v*(y-1)/N))); % add term
end
end
end
end
Fab=abs(F);
figure;imshow(Fab,[0 1],'InitialMagnification','fit')
%Inverse Transform
z = zeros(M,N);
for x=0:1:M-1
for y=0:1:N-1
for u=1:1:M
for v=1:1:N
z(x+1,y+1) = z(x+1,y+1) + (1/(M*N)) * ... % corrected scale factor
F(u,v)*exp(2*pi*(1i)*(((u-1)*x/M)+((v-1)*y/N))); % add term
end
end
end
end
figure;imshow(real(z),[0 1],'InitialMagnification','fit')
Now the original and recovered image differ only by very small values, of the order of eps, due to the usual floating-point inaccuacies:
>> f-z
ans =
1.0e-15 *
Columns 1 through 2
0.180411241501588 + 0.666133814775094i -0.111022302462516 - 0.027755575615629i
0.000000000000000 + 0.027755575615629i 0.277555756156289 + 0.212603775716506i
0.000000000000000 - 0.194289029309402i 0.000000000000000 + 0.027755575615629i
Column 3
-0.194289029309402 - 0.027755575615629i
-0.222044604925031 - 0.055511151231258i
0.111022302462516 - 0.111022302462516i
Firstly, the biggest error is that you are computing the Fourier transform incorrectly. When computing F, you need to be summing over x and y, which you are not doing. Here's how to rectify that:
F = zeros(M, N);
for u=0:1:M-1
for v=0:1:N-1
for x=1:1:M
for y=1:1:N
F(u+1,v+1)=F(u+1,v+1) + f(x,y)*exp(-2*pi*(1i)*((u*(x-1)/M)+(v*(y-1)/N)));
end
end
end
end
Secondly, in the inverse transform, your bracketing is incorrect. It should be 1/(M*N) not (1/M*N).
As an aside, at the cost of a bit more memory, you can speed up the computation by not nesting so many loops. Namely, when computing the FFT, do the following instead
x = (1:1:M)'; % x is a column vector
y = (1:1:N) ; % y is a row vector
for u = 0:1:M-1
for v = 0:1:N-1
F2(u+1,v+1) = sum(f .* exp(-2i * pi * (u*(x-1)/M + v*(y-1)/N)), 'all');
end
end
To take this method to the extreme, i.e. not using any loops at all, you would do the following (though this is not recommended, since you would lose code readability and the memory cost would increase exponentially)
x = (1:1:M)'; % x is in dimension 1
y = (1:1:N) ; % y is in dimension 2
u = permute(0:1:M-1, [1, 3, 2]); % x-freqs in dimension 3
v = permute(0:1:N-1, [1, 4, 3, 2]); % y-freqs in dimension 4
% sum the exponential terms in x and y, which are in dimensions 1 and 2.
% If you are using r2018a or older, the below summation should be
% sum(sum(..., 1), 2)
% instead of
% sum(..., [1,2])
F3 = sum(f .* exp(-2i * pi * (u.*(x-1)/M + v.*(y-1)/N)), [1, 2]);
% The resulting array F3 is 1 x 1 x M x N, to make it M x N, simply shiftdim or squeeze
F3 = squeeze(F3);
I want to solve the following system of equations shown in the image below,
The matrix system
where the component of the matrix A is complex numbers with the angle (theta) runs from 0 to 2*pi which has m divisions, and n = 9. The known value z = x + iy. Suppose the x and y of matrix z is
z =
0 1.0148
0.1736 0.9848
0.3420 0.9397
0.5047 0.8742
0.6748 0.8042
0.8419 0.7065
0.9919 0.5727
1.1049 0.4022
1.1757 0.2073
1.1999 0
1.1757 -0.2073
1.1049 -0.4022
0.9919 -0.5727
0.8419 -0.7065
0.6748 -0.8042
0.5047 -0.8742
0.3420 -0.9397
0.1736 -0.9848
0 -1.0148
How do you solve them iteratively? Notice that the value of the first component of the desired constants must equal 1. I am working with Matlab.
You can apply simple multilinear regression for complex valued data.
Step 1. Get the matrix ready for linear regression
Your linear system
written without matrices, becomes
that rearranged yelds
If you rewrite it with matrices you get
Step 2. Apply multiple linear regression
Let the system above be
where
Now you can apply linear regression, that returns the best fit for α when
where
is the conjugate transpose.
In MATLAB
Y = Z - A(:,1); % Calculate Y subtracting the first col of A from Z
R = A(:,:); R(:,1) = []; % Calculate R as an exact copy of A, just without first column
Rs = ctranspose(R); % Calculate R-star (conjugate transpose of R)
alpha = (Rs*R)^(-1)*Rs*Y; % Finally apply multiple linear regression
alpha = cat(1, 1, alpha); % Add alpha1 back, whose value is 1
or, if you prefer built-ins, have a look at regress function:
Y = Z - A(:,1); % Calculate Y subtracting the first col of A from Z
R = A(:,:); R(:,1) = []; % Calculate R as an exact copy of A, just without first column
alpha = regress(Y, R); % Finally apply multiple linear regression
alpha = cat(1, 1, alpha); % Add alpha1 back, whose value is 1
We were asked to define our own differential operators on MATLAB, and I did it following a series of steps, and then we should use the differential operators to solve a boundary value problem:
-y'' + 2y' - y = x, y(0) = y(1) =0
my code was as follows, it was used to compute this (first and second derivative)
h = 2;
x = 2:h:50;
y = x.^2 ;
n=length(x);
uppershift = 1;
U = diag(ones(n-abs(uppershift),1),uppershift);
lowershift = -1;
L= diag(ones(n-abs(lowershift),1),lowershift);
% the code above creates the upper and lower shift matrix
D = ((U-L))/(2*h); %first differential operator
D2 = (full (gallery('tridiag',n)))/ -(h^2); %second differential operator
d1= D*y.'
d2= ((D2)*y.')
then I changed it to this after posting it here and getting one response that encouraged the usage of Identity Matrix, however I still seem to be getting no where.
h = 2;
n=10;
uppershift = 1;
U = diag(ones(n-abs(uppershift),1),uppershift);
lowershift = -1;
L= diag(ones(n-abs(lowershift),1),lowershift);
D = ((U-L))/(2*h); %first differential operator
D2 = (full (gallery('tridiag',n)))/ -(h^2); %second differential operator
I= eye(n);
eqn=(-D2 + 2*D - I)*y == x
solve(eqn,y)
I am not sure how to proceed with this, like should I define y and x, or what exactly? I am clueless!
Because this is a numerical approximation to the solution of the ODE, you are seeking to find a numerical vector that is representative of the solution to this ODE from time x=0 to x=1. This means that your boundary conditions make it so that the solution is only valid between 0 and 1.
Also this is now the reverse problem. In the previous post we did together, you know what the input vector was, and doing a matrix-vector multiplication produced the output derivative operation on that input vector. Now, you are given the output of the derivative and you are now seeking what the original input was. This now involves solving a linear system of equations.
Essentially, your problem is now this:
YX = F
Y are the coefficients from the matrix derivative operators that you derived, which is a n x n matrix, X would be the solution to the ODE, which is a n x 1 vector and F would be the function you are associating the ODE with, also a n x 1 vector. In our case, that would be x. To find Y, you've pretty much done that already in your code. You simply take each matrix operator (first and second derivative) and you add them together with the proper signs and scales to respect the left-hand side of the ODE. BTW, your first derivative and second derivative matrices are correct. What's left is adding the -y term to the mix, and that is accomplished by -eye(n) as you have found out in your code.
Once you formulate your Y and F, you can use the mldivide or \ operator and solve for X and get the solution to this linear system via:
X = Y \ F;
The above essentially solves the linear system of equations formed by Y and F and will be stored in X.
The first thing you need to do is define a vector of points going from x=0 to x=1. linspace is probably the most suitable where you can specify how many points we want. Let's assume 100 points for now:
x = linspace(0,1,100);
Therefore, h in our case is just 1/100. In general, if you want to solve from the starting point x = a up to the end point x = b, the step size h is defined as h = (b - a)/n where n is the total number of points you want to solve for in the ODE.
Now, we have to include the boundary conditions. This simply means that we know the beginning and ending of the solution of the ODE. This means that y(0) = y(1) = 0. As such, we make sure that the first row of Y has only the first column set to 1 and the last row of Y has only the last column set to 1, and we'll set the output position in F to both be 0. This symbolizes that we already know the solution at these points.
Therefore, your final code to solve is just:
%// Setup
a = 0; b = 1; n = 100;
x = linspace(a,b,n);
h = (b-a)/n;
%// Your code
uppershift = 1;
U = diag(ones(n-abs(uppershift),1),uppershift);
lowershift = -1;
L= diag(ones(n-abs(lowershift),1),lowershift);
D = ((U-L))/(2*h); %first differential operator
D2 = (full (gallery('tridiag',n)))/ -(h^2);
%// New code - Create differential equation matrix
Y = (-D2 + 2*D - eye(n));
%// Set boundary conditions on system
Y(1,:) = 0; Y(1,1) = 1;
Y(end,:) = 0; Y(end,end) = 1;
%// New code - Create F vector and set boundary conditions
F = x.';
F(1) = 0; F(end) = 0;
%// Solve system
X = Y \ F;
X should now contain your numerical approximation to the ODE in steps of h = 1/100 starting from x=0 up to x=1.
Now let's see what this looks like:
figure;
plot(x, X);
title('Solution to ODE');
xlabel('x'); ylabel('y');
You can see that y(0) = y(1) = 0 as per the boundary conditions.
Hope this helps, and good luck!
I am actually attempting to write code for the cubic spline interpolation. Cubic spline boils down to a series of n-1 segments where n is the number of original coordinates given initially and the segments are each represented by some cubic function.
I have figured out how to get all the coefficients and values for each segment, stored in vectors a,b,c,d, but I don't know how to plot the function as a piecewise function on different intervals. Here is my code so far. The very last for loop is where I have attempted to plot each segment.
%initializations
x = [1 1.3 1.9 2.1 2.6 3.0 3.9 4.4 4.7 5.0 6 7 8 9.2 10.5 11.3 11.6 12 12.6 13 13.3].';
y = [1.3 1.5 1.85 2.1 2.6 2.7 2.4 2.15 2.05 2.1 2.25 2.3 2.25 1.95 1.4 0.9 0.7 0.6 0.5 0.4 0.25].';
%n is the amount of coordinates
n = length(x);
%solving for a-d for all n-1 segments
a = zeros(n,1);
b = zeros(n,1);
d = zeros(n,1);
%%%%%%%%%%%%%% SOLVE FOR a's %%%%%%%%%%%%%
%Condition (b) in Definition 3.10 on pg 146
%Sj(xj) = f(xj) aka yj
for j = 1: n
a(j) = y(j);
end
%initialize hj
h = zeros(n-1,1);
for j = 1: n-1
h(j) = x(j+1) - x(j);
end
A = zeros(n,n);
bv = zeros(n,1); %bv = b vector
%initialize corners to 1
A(1,1) = 1;
A(n,n) = 1;
%set main diagonal
for k = 2: n-1
A(k,k) = 2*(h(k-1) + h(k));
end
%set upper and then lower diagonals
for k = 2 : n-1
A(k,k+1) = h(k); %h2, h3, h4...hn-1
A(k,k-1) = h(k-1); %h1, h2, h3...hn
end
%fill up the b vector using equation in notes
%first and last spots are 0
for j = 2 : n-1
bv(j) = 3*(((a(j+1)-a(j)) / h(j)) - ((a(j) - a(j-1)) / h(j-1)));
end
%augmented matrix
A = [A bv];
%%%%%%%%%%%% BEGIN GAUSSIAN ELIMINATION %%%%%%%%%%%%%%%
offset = 1;
%will only need n-1 iterations since "first" pivot row is unchanged
for k = 1: n-1
%Searching from row p to row n for non-zero pivot
for p = k : n
if A(p,k) ~= 0;
break;
end
end
%row swapping using temp variable
if p ~= k
temp = A(p,:);
A(p,:) = A(k,:);
A(k,:) = temp;
end
%Eliminations to create Upper Triangular Form
for j = k+1:n
A(j,offset:n+1) = A(j,offset:n+1) - ((A(k, offset:n+1) * A(j,k)) / A(k,k));
end
offset = offset + 1;
end
c = zeros(n,1); %initializes vector of data of n rows, 1 column
%Backward Subsitution
%First, solve the nth equation
c(n) = A(n,n+1) / A(n,n);
%%%%%%%%%%%%%%%%% SOLVE FOR C's %%%%%%%%%%%%%%%%%%
%now solve the n-1 : 1 equations (the rest of them going backwards
for j = n-1:-1:1 %-1 means decrement
c(j) = A(j,n+1);
for k = j+1:n
c(j) = c(j) - A(j,k)*c(k);
end
c(j) = c(j)/A(j,j);
end
%%%%%%%%%%%%% SOLVE FOR B's and D's %%%%%%%%%%%%%%%%%%%%
for j = n-1 : -1 : 1
b(j) = ((a(j+1)-a(j)) / h(j)) - (h(j)*(2*c(j) + c(j+1)) / 3);
d(j) = (c(j+1) - c(j)) / 3*h(j);
end
%series of equation segments
for j = 1 : n-1
f = #(x) a(j) + b(j)*(x-x(j)) + c(j)*(x-x(j))^2 + d(j)*(x-x(j))^3;
end
plot(x,y,'o');
Let's assume that I have calculated vectors a,b,c,d correctly for each segment. How do I plot each cubic segment such that they all appear graphed on a single plot?
I appreciate the help.
That's pretty easy. You've already done half of the work by defining an anonymous function that is for the cubic spline in between each interval. However, you need to make sure that the operations in the function are element-wise. You currently have it operating on scalars, or assuming that we are using matrix operations. Don't do that. Use .* instead of * and .^ instead of ^. The reason why you need to do this is to make generating the points on the spline a lot easier, where my next point follows.
All you have to do next is define a bunch of x points within the interval defined by the neighbouring x key points and substitute them into your function, then plot the result.... so something like this:
figure;
hold on;
for j = 1 : n-1
f = #(x) a(j) + b(j).*(x-x(j)) + c(j).*(x-x(j)).^2 + d(j)*(x-x(j)).^3; %// Change function to element-wise operations - be careful
x0 = linspace(x(j), x(j+1)); %// Define set of points
y0 = f(x0); %// Find output points
plot(x0, y0, 'r'); %// Plot the line in between the key points
end
plot(x, y, 'bo');
We spawn a new figure, then use hold on so that when we call plot multiple times, we append the results to the same figure. Next, for each set of cubic spline coefficients we have, define a spline function, then generate a bunch of x values with linspace that are between the current x key point and the one beside it. By default, linspace generates 100 points between a start point (i.e. x(j)) and end point (i.e. x(j+1)). You can control how many points you want to generate by specifying a third parameter (so something like linspace(x(j), x(j+1), 25); to generate 25 points). We use these x values and substitute them into our spline equation to get our y values. We then plot this result on the figure using a red line. Once we're done, we plot the key points as blue open circles on top of the curve.
As a bonus, I ran your code with the above plotting mechanism, and this is what I get:
We are given a D-dimensional tensor, represented as a vector of size n^D.
The vector represents a D-dimensional distribution of a random variable X \in {0,1,..,n}^d. That is the (i_1,i_2,...,i_d) entry in the tensor represents the probability of X_1 = i_1, X_2 = i_2, ... X_d = i_d.
I need to compute, for each dimension d, and value i\in [n] the marginal distribution P(X_d = i).
i.e., this means that the answer of P(X_d = i) is the sum of n^(D-1) entries of the vector.
For example, if D=2 and n=4, we have a vector x of size (16,1) and the probability of the first dimension being equal to 1 is
P(X_1 = 1) = x(1) + x(2) + x(3) + x(4)
The probability of the second dimension being equal to 3 is '
P(X_2 = 3) = x(3) + x(7) + x(11) + x(15)
I'm writing Matlab code that needs to compute these marginal distributions, but I'm not familiar enough with Matlab to do it in a simple way (it is doable using some ugly recursion, but there has to be a better option).
To calculate P(X_k=z) for a D-dimensional matrix you can use
xD = reshape(x, n*ones(1,D));
B = permute(xD, [k setdiff(1:D, k)]);
P = sum(B(z,:));
It first makes it a D-dimensional matrix. It brings the dimension of interest k to the beginning and then chooses the z-th element and sums over elements corresponding to that.
Mohsen Nosratinia's answer would be my first option. As an alternative, it can be done without reshaping or permuting dimensions, which can result in faster code:
k = 2; %// chosen dimension
z = 3; %// chosen value (along d-th dimension)
result = sum(x(mod(floor((0:end-1)/n^(k-1)), n)==z-1));