Related
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.
I have the following system, specified by the set of coefficients:
b = [1 2 3];
a = [1 .5 .25];
In the Z-Domain, such function will have the following transfer function:
H(Z) = Y(Z)/X(Z)
So the frequency response will be just the unit circle, where:
H(e^jw) = Y(e^jw)/X(e^jw)
Do I just substitute in the e^jw for 'Z' in my transfer function to obtain the frequency response of the system mathematically, on paper? Seems a bit ridiculous from my (a student's) point of view.
Have you tried freqz()? It returns the frequency response vector, h, and the corresponding angular frequency vector, w, for the digital filter with numerator and denominator polynomial coefficients stored in b and a, respectively.
In your case, simply follow the help:
[h,w]=freqz(b,a);
You do sub in e^jw for Z. This isn't ridiculous. Then you just sweep w from -pi to pi. Your freq response will be the absolute value of the result.
As Alessiox mentioned, freqz is the command you want to use in matlab.
I would indeed be as simple as substituting exp(j*w) in your transfer function. There are of course different ways to implement this with Matlab. For the purpose of illustration, I will be assuming bs are the coefficients of the x sequence and as are the coefficients of the y sequence, such that the b are in the numerator and the as are in the denominator:
A direct evaluation with Matlab could be done with:
b = [1 2 3];
a = [1 .5 .25];
N = 513; % number of points at which to evaluate the transfer function
w = linspace(0,2*pi,N);
num = 0;
for i=1:length(b)
num = num + b(i) * exp(-j*i*w);
end
den = 0;
for i=1:length(a)
den = den + a(i) * exp(-j*i*w);
end
H = num ./ den;
This would be equivalent to the following which makes use of the builtin polyval:
N = 513; % number of points at which to evaluate the transfer function
w = linspace(0,2*pi,N);
H = polyval(fliplr(b),exp(-j*w))./polyval(fliplr(a),exp(-j*w));
Also, this is really evaluating the transfer function at discrete equally spaced angular frequencies w = 2*pi*k/N which corresponds to the Discrete Fourier Transform (DFT). As such it could also be done with:
N = 512;
H = fft(b,N) ./ fft(a,N);
Incidentally this is what freqz does, so you could also get the same result with:
N = 512;
H = freqz(b,a,N,'whole');
I'm trying to get Matlab to take this as a function of x_1 through x_n and y_1 through y_n, where k_i and r_i are all constants.
So far my idea was to take n from the user and make two 1×n vectors called x and y, and for the x_i just pull out x(i). But I don't know how to make an arbitrary sum in MATLAB.
I also need to get the gradient of this function, which I don't know how to do either. I was thinking maybe I could make a loop and add that to the function each time, but MATLAB doesn't like that.
I don't believe a loop is necessary for this calculation. MATLAB excels at vectorized operations, so would something like this work for you?
l = 10; % how large these vectors are
k = rand(l,1); % random junk values to work with
r = rand(l,1);
x = rand(l,1);
y = rand(l,1);
vals = k(1:end-1) .* (sqrt(diff(x).^2 + diff(y).^2) - r(1:end-1)).^2;
sum(vals)
EDIT: Thanks to #Amro for correcting the formula and simplifying it with diff.
You can solve for the gradient symbolically with:
n = 10;
k = sym('k',[1 n]); % Create n variables k1, k2, ..., kn
x = sym('x',[1 n]); % Create n variables x1, x2, ..., xn
y = sym('y',[1 n]); % Create n variables y1, y2, ..., yn
r = sym('r',[1 n]); % Create n variables r1, r2, ..., rn
% Symbolically sum equation
s = sum((k(1:end-1).*sqrt((x(2:end)-x(1:end-1)).^2+(y(2:end)-y(1:end-1)).^2)-r(1:end-1)).^2)
grad_x = gradient(s,x) % Gradient with respect to x vector
grad_y = gradient(s,y) % Gradient with respect to y vector
The symbolic sum and gradients can be evaluated and converted to floating point with:
% n random data values for k, x, y, and r
K = rand(1,n);
X = rand(1,n);
Y = rand(1,n);
R = rand(1,n);
% Substitute in data for symbolic variables
S = double(subs(s,{[k,x,y,r]},{[K,X,Y,R]}))
GRAD_X = double(subs(grad_x,{[k,x,y,r]},{[K,X,Y,R]}))
GRAD_Y = double(subs(grad_y,{[k,x,y,r]},{[K,X,Y,R]}))
The gradient function is the one overloaded for symbolic variables (type help sym/gradient) or see the more detailed documentation online).
Yes, you could indeed do this with a loop, considering that x, y, k, and r are already defined.
n = length(x);
s = 0;
for j = 2 : n
s = s + k(j-1) * (sqrt((x(j) - x(j-1)).^2 + (y(j) - y(j-1)).^2) - r(j-1)).^2
end
You should derive the gradient analytically and then plug in numbers. It should not be too hard to expand these terms and then find derivatives of the resulting polynomial.
Vectorized solution is something like (I wonder why do you use sqrt().^2):
is = 2:n;
result = sum( k(is - 1) .* abs((x(is) - x(is-1)).^2 + (y(is) - y(is-1)).^2 - r(is-1)));
You can either compute gradient symbolically or rewrite this code as a function and make a standard +-eps calculation. If you need a gradient to run optimization (you code looks like a fitness function) you could use algorithms that calculate them themselves, for example, fminsearch can do this
Given 100x100 matrix where each element represents a function value in space, I would like to find parameter values A, B, C, D, E for a function f(x, y) = A + Bx + Cy + DX^2+Ey^2 that fits the best the given matrix values, where x represents a row number and y represents a column number
To illustrate the aim on a smaller example, let's say we have a 3x3 matrix T:
T = [0.1 0.2 0.1; 0.8, 0.6, 0.5; 0.1, 0, 1]
in this case f(1,1) = 0.1 and f(3,2)= 0.
Concretely the matrix values for which I would like to find a fitting function (surface) are displayed in the image below:
I would be very thankful if anyone suggested a way to find the 3D function that fits (best) the given matrix.
Edit
Is it possible to find a fit directly or is it neccesary (or better) fo first represent the data as matrix [X, Y, f(X,Y)]:
vals = []
for(i = 1: 100)
for j = 1 : 100
if(T(i,j) ~= 0)
vals = [vals;i, j, T(i,j)];
end;
end;
end;
These guys seemed to have done it in one line:
http://www.mathworks.com/matlabcentral/newsreader/view_thread/134076
x = % vector of x values
y = % vector of y values
z = % f(x,y)
V = [1 x y x.^2 x.*y y.^2];
a = V \ z ;
From the help page:
If A is a rectangular m-by-n matrix with m ~= n, and B is a column vector with m elements or a matrix with m rows, then A\B returns a least-squares solution to the system of equations A*x= B.
I have double summation over m = 1:M and n = 1:N for polar point with coordinates rho, phi, z:
I have written vectorized notation of it:
N = 10;
M = 10;
n = 1:N;
m = 1:M;
rho = 1;
phi = 1;
z = 1;
summ = cos (n*z) * besselj(m'-1, n*rho) * cos(m*phi)';
Now I need to rewrite this function for accepting vectors (columns) of coordinates rho, phi, z. I tried arrayfun, cellfun, simple for loop - they work too slow for me. I know about "MATLAB array manipulation tips and tricks", but as MATLAB beginner I can't understand repmat and other functions.
Can anybody suggest vectorized solution?
I think your code is already well vectorized (for n and m). If you want this function to also accept an array of rho/phi/z values, I suggest you simply process the values in a for-loop, as I doubt any further vectorization will bring significant improvements (plus the code will be harder to read).
Having said that, in the code below, I tried to vectorize the part where you compute the various components being multiplied {row N} * { matrix N*M } * {col M} = {scalar}, by making a single call to the BESSELJ and COS functions (I place each of the row/matrix/column in the third dimension). Their multiplication is still done in a loop (ARRAYFUN to be exact):
%# parameters
N = 10; M = 10;
n = 1:N; m = 1:M;
num = 50;
rho = 1:num; phi = 1:num; z = 1:num;
%# straightforward FOR-loop
tic
result1 = zeros(1,num);
for i=1:num
result1(i) = cos(n*z(i)) * besselj(m'-1, n*rho(i)) * cos(m*phi(i))';
end
toc
%# vectorized computation of the components
tic
a = cos( bsxfun(#times, n, permute(z(:),[3 2 1])) );
b = besselj(m'-1, reshape(bsxfun(#times,n,rho(:))',[],1)'); %'
b = permute(reshape(b',[length(m) length(n) length(rho)]), [2 1 3]); %'
c = cos( bsxfun(#times, m, permute(phi(:),[3 2 1])) );
result2 = arrayfun(#(i) a(:,:,i)*b(:,:,i)*c(:,:,i)', 1:num); %'
toc
%# make sure the two results are the same
assert( isequal(result1,result2) )
I did another benchmark test using the TIMEIT function (gives more fair timings). The result agrees with the previous:
0.0062407 # elapsed time (seconds) for the my solution
0.015677 # elapsed time (seconds) for the FOR-loop solution
Note that as you increase the size of the input vectors, the two methods will start to have similar timings (the FOR-loop even wins on some occasions)
You need to create two matrices, say m_ and n_ so that by selecting element i,j of each matrix you get the desired index for both m and n.
Most MATLAB functions accept matrices and vectors and compute their results element by element. So to produce a double sum, you compute all elements of the sum in parallel by f(m_, n_) and sum them.
In your case (note that the .* operator performs element-wise multiplication of matrices)
N = 10;
M = 10;
n = 1:N;
m = 1:M;
rho = 1;
phi = 1;
z = 1;
% N rows x M columns for each matrix
% n_ - all columns are identical
% m_ - all rows are identical
n_ = repmat(n', 1, M);
m_ = repmat(m , N, 1);
element_nm = cos (n_*z) .* besselj(m_-1, n_*rho) .* cos(m_*phi);
sum_all = sum( element_nm(:) );