i have trouble getting a matlab code to work properly! i found a cubic spline code in matlab to give me the interpolated polynomial. and i simply give it an example to work:
Xi = [0 0.05 0.1]
Fi = [1 1.105171 1.221403]
Fi' = [2 _ 2.442806]
but it gives me this error:
??? Attempted to access du(1); index out of bounds because numel(du)=0.
Error in ==> cubic_nak at 53
du(1) = du(1) - hi(1)^2 / hi(2);
here is the full code for not a knot condition
function csn = cubic_nak ( xi, fi )
%CUBIC_NAK compute the cubic spline interpolant, subject to
% "not-a-knot" boundary conditions, associated with a
% given set of interpolating points and function values
%
% calling sequences:
% csn = cubic_nak ( xi, fi )
% cubic_nak ( xi, fi )
%
% inputs:
% xi vector containing the interpolating points
% (must be sorted in ascending order)
% fi vector containing function values
% the i-th entry in this vector is the function
% value associated with the i-th entry in the 'xi'
% vector
%
% output:
% csn five column matrix containing the information which
% defines the "not-a-knot" cubic spline interpolant
% - first column: interpolating points
% - second column: function values
% - third column: coefficients of linear terms
% - fourth column: coefficients of quadratic terms
% - fifth column: coefficients of cubic terms
%
% NOTE:
% to evaluate the "not-a-knot" cubic spline interpolant apply
% the routine SPLINE_EVAL to the output of this routine
%
n = length ( xi );
m = length ( fi );
if ( n ~= m )
disp ( 'number of ordinates and number of function values must be equal' )
return
end
for i = 1 : n-1
hi(i) = xi(i+1) - xi(i);
end
for i = 1 : n-2
dd(i) = 2.0 * ( hi(i) + hi(i+1) );
ri(i) = (3.0/hi(i+1))*(fi(i+2)-fi(i+1))-(3.0/hi(i))*(fi(i+1)-fi(i));
end
dd(1) = dd(1) + hi(1) + hi(1)^2 / hi(2);
dd(n-2) = dd(n-2) + hi(n-1) + hi(n-1)^2 / hi(n-2);
du = hi(2:n-2);
dl = du;
du(1) = du(1) - hi(1)^2 / hi(2);
dl(n-3) = dl(n-3) - hi(n-1)^2 / hi(n-2);
temp = tridiagonal ( dl, dd, du, ri );
c = zeros ( n,1 );
d = c; b = c;
c(2:n-1) = temp;
c(1) = ( 1 + hi(1) / hi(2) ) * c(2) - hi(1) / hi(2) * c(3);
c(n) = ( 1 + hi(n-1) / hi(n-2) ) * c(n-1) - hi(n-1) / hi(n-2) * c(n-2);
for i = 1 : n-1
d(i) = (c(i+1)-c(i))/(3.0*hi(i));
b(i) = (fi(i+1)-fi(i))/hi(i) - hi(i)*(c(i+1)+2.0*c(i))/3.0;
end
if ( nargout == 0 )
disp ( [ xi' fi' b c d ] )
else
csn = [ xi' fi' b c d ];
end
also for the clamped condition it gives me this error:
??? Undefined function or method 'tridiagonal' for input arguments of type 'double'.
Error in ==> cubic_clamped at 55
c = tridiagonal ( hi(1:n-1), dd, hi(1:n-1), ri );
??? Input argument "xi" is undefined.
Error in ==> cubic_clamped at 35
n = length ( xi );
the full code for clamped mode:
function csc = cubic_clamped ( xi, fi, fpa, fpb )
%CUBIC_CLAMPED compute the cubic spline interpolant, subject to
% "clamped" boundary conditions, associated with a
% given set of interpolating points and function values
%
% calling sequences:
% csc = cubic_clamped ( xi, fi, fpa, fpb )
% cubic_clamped ( xi, fi, fpa, fpb )
%
% inputs:
% xi vector containing the interpolating points
% (must be sorted in ascending order)
% fi vector containing function values
% the i-th entry in this vector is the function
% value associated with the i-th entry in the 'xi'
% vector
% fpa derivative value at left endpoint; i.e., xi(1)
% fpb derivative value at right endpoint; i.e., xi(n)
%
% output:
% csn five column matrix containing the information which
% defines the "clamped" cubic spline interpolant
% - first column: interpolating points
% - second column: function values
% - third column: coefficients of linear terms
% - fourth column: coefficients of quadratic terms
% - fifth column: coefficients of cubic terms
%
% NOTE:
% to evaluate the "clamped" cubic spline interpolant apply
% the routine SPLINE_EVAL to the output of this routine
%
n = length ( xi );
m = length ( fi );
if ( n ~= m )
disp ( 'number of ordinates and number of function values must be equal' )
return
end
for i = 1 : n-1
hi(i) = xi(i+1) - xi(i);
end
dd(1) = 2.0*hi(1); dd(n) = 2.0*hi(n-1);
ri(1) = (3.0/hi(1))*(fi(2)-fi(1)) - 3.0 * fpa;
ri(n) = 3.0 * fpb - (3.0/hi(n-1))*(fi(n)-fi(n-1));
for i = 1 : n-2
dd(i+1) = 2.0 * ( hi(i) + hi(i+1) );
ri(i+1) = (3.0/hi(i+1))*(fi(i+2)-fi(i+1))-(3.0/hi(i))*(fi(i+1)-fi(i));
end
disp ( [dd' ri'] )
c = tridiagonal ( hi(1:n-1), dd, hi(1:n-1), ri );
d = zeros ( n,1 );
b = d;
for i = 1 : n-1
d(i) = (c(i+1)-c(i))/(3.0*hi(i));
b(i) = (fi(i+1)-fi(i))/hi(i) - hi(i)*(c(i+1)+2.0*c(i))/3.0;
end
if ( nargout == 0 )
disp ( [ xi' fi' b c' d ] )
else
csc = [ xi' fi' b c' d ];
end
for this one it only gives me the first 2 columns!
so anyone knows how i can make these 2 work?
Your data only has 3 points. You need 4 (or more) points to fit a cubic, so your out-of-bounds error probably comes from the code looking for another point in the array.
Related
I want to multiply two matrices, the first matrix is the inverse of A, and the second matrix is B,
input('Enter The first Matrix')
A = [ 1 2 3 ; 4 5 6 ; 7 8 0 ]
[m n] = size(A)
if m==n
if det(A)==0
disp('inverse does not exist')
else
invv=inv(A)
disp(invv)
end
else
disp('Number of rows and columns are not equel , no inverse')
end
input('Enter The second Matrix')
B = [ 1 ; 1 ; 1 ]
How can I verify that the number of columns in the first matrix is equal to the number of rows in the second matrix, so that they can be multiplied?
function [X] = dot_inv(X,x)
% X - numeric array
% x - cell array of numeric vectors
% DIM - dimensions to omit (asumes ndims(X) = numel(x))
% Y - inner product obtained by summing the products of X and x along DIM
% initialise dimensions
%--------------------------------------------------------------------------
[m n] = size(X)
if m==n
if det(X)==0
error('Error. \n inverse does not exist.')
else
X=inv(X)
end
else
error('Error. \n Number of rows and columns are not equel , no inverse!')
end
if iscell(x)
DIM = (1:numel(x)) + ndims(X) - numel(x);
else
DIM = 1;
x = {x};
end
% inner product using recursive summation (and bsxfun)
%--------------------------------------------------------------------------
for d = 1:numel(x)
s = ones(1,ndims(X));
s(DIM(d)) = numel(x{d});
X = bsxfun(#times,X,reshape(full(x{d}),s));
X = sum(X,DIM(d));
end
% eliminate singleton dimensions
%--------------------------------------------------------------------------
X = squeeze(X);
return
I tried to implement the cost function of the Dual Absolute Quadric in Matlab according to the following equation mentioned in this paper, with this data.
My problem is that the results didn't converge.
The code is down.
main code
%---------------------
% clear and close all
%---------------------
clearvars
close all
clc
%---------------------
% Data type long
%---------------------
format long g
%---------------------
% Read data
%---------------------
load('data.mat')
%---------------------------
% Display The Initial Guess
%---------------------------
disp('=======================================================')
disp('Initial Intrinsic parameters: ');
disp(A);
disp('xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx')
%=========================================================================
DualAbsoluteQuadric = Optimize(A,#DAQ);
%---------------------
% Display The Results
%---------------------
disp('=======================================================')
disp('Dual Absoute Quadric cost function: ');
disp(DualAbsoluteQuadric);
disp('xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx')
The optimization function used is:
function output = Optimize(A,func)
%------------------------------
options = optimoptions('lsqnonlin','Algorithm','levenberg-marquardt',...
'Display','iter','FunctionTolerance',1e-16,'Tolx',1e-16,...
'MaxFunctionEvaluations', 1000, 'MaxIterations',39,...
'OptimalityTolerance',1e-16);
%------------------------------
% NonLinear Optimization
%------------------------------
output_line = lsqnonlin(func,[A(1,1), A(1,3), A(2,2), A(2,3), A(1,2)],...
[],[],options);
%------------------------------------------------------------------------
output = Reshape(output_line);
The Dual Absolute Quadric Function:
function cost = DAQ(params)
Aj = [params(1) params(5) params(2) ;
0 params(3) params(4) ;
0 0 1 ];
Ai = [params(1) params(5) params(2) ;
0 params(3) params(4) ;
0 0 1 ];
% W^-1 (IAC Image of the Absolute Conic)
W_inv = Ai * Aj';
%----------------
%Find plane at infinity from MQM' ~ w (Dual Absolute Quadric)
Plane_at_infinity = PlaneAtInfinity(W_inv);
%Find H_Infty = [e21]F+e21*n'
Homography_at_infty = H_Infty(Plane_at_infinity);
%----------------
% Initialization
%----------------
global Fs;
% Initialize the cost as a vector
% (N-1 * N-2)/2: 9*8/2 = 36
vector_size = (size(Fs,3)-1)*(size(Fs,4)-2)/2;
cost = zeros(1, vector_size);
% Cost Function
k = 0;
loop_size = 3 * vector_size;
Second_Term = W_inv / norm(W_inv,'fro');
for i=1:3:loop_size
k = k+1;
First_Term = Homography_at_infty(:,i:i+2) * W_inv * ((Homography_at_infty(:,i:i+2))');
First_Term = First_Term / norm(First_Term, 'fro');
cost(k) = norm(First_Term - Second_Term,'fro');
end
end
Plane at infinity function:
function P_infty = PlaneAtInfinity(W_inv)
global PPM;
% Symbolic variables
X = sym('X', 'real');
Y = sym('Y', 'real');
Z = sym('Z', 'real');
L2 = sym('L2','real');
n = [X; Y; Z];
% DAQ
Q = [W_inv , (W_inv * n) ;
(n' * W_inv) , (n' * W_inv * n)];
% Get one only camera matrix (any)
M = PPM(:, :, 3);
% Autocalibration equation
m = M * Q * M';
% solve linear equations
solution = solve(m(1, 1) == (L2 * W_inv(1, 1)), ...
m(2, 2) == (L2 * W_inv(2, 2)), ...
m(3, 3) == (L2 * W_inv(3, 3)), ...
m(1, 3) == (L2 * W_inv(1, 3)));
P_infty = [double(solution.X(1)) double(solution.Y(1))...
double(solution.Z(1))]';
Homography at infinity function:
function H_Inf = H_Infty(planeInf)
global Fs;
k = 1;
% (3 x 3) x ((N-1)*(N-2) /2)
H_Inf = zeros(3,3*(size(Fs,3)-1)*(size(Fs,4)-2)/2);%(3*3)*36
for i = 2:size(Fs,3)
for j = i+1:size(Fs,4)
[~, ~, V] = svd(Fs(:,:,i,j)');
epip = V(:,end);
H_Inf(:,k:k+2) = epipole(Fs(:,:,i,j)) * Fs(:,:,i,j)+ epip * planeInf';
k = k+3;
end
end
end
Reshape function:
function output = Reshape(input)
%---------------------
% Reshape Intrinsics
%---------------------
% K = [a skew u0 ;
% 0 B v0 ;
% 0 0 1 ];
output = [input(1) input(5) input(2) ;
0 input(3) input(4) ;
0 0 1 ];
end
Epipole Function:
function epip = epipole(Fs)
% SVD Decompostition of (Fs)^T
[~,~,V] = svd(Fs');
% Get the epipole from the last vector of the SVD
epi = V(:,end);
% Reshape the Vector into Matrix
epip = [ 0 -epi(3) epi(2);
epi(3) 0 -epi(1);
-epi(2) epi(1) 0 ];
end
The plane at infinity has to be calculated as following:
function plane = computePlaneAtInfinity(P, K)
%Input
% P - Projection matrices
% K - Approximate Values of Intrinsics
%
%Output
% plane - coordinate of plane at infinity
% Compute the DIAC W^-1
W_invert = K * K';
% Construct Symbolic Variables to Solve for Plane at Infinity
% X,Y,Z is the coordinate of plane at infinity
% XX is the scale
X = sym('X', 'real');
Y = sym('Y', 'real');
Z = sym('Z', 'real');
XX = sym('XX', 'real');
% Define Normal to Plane at Infinity
N = [X; Y; Z];
% Equation of Dual Absolute Quadric (DAQ)
Q = [W_invert, (W_invert * N); (N' * W_invert), (N' * W_invert * N)];
% Select Any One Projection Matrix
M = P(:, :, 2);
% Left hand side of the equation
LHS = M * Q * M';
% Solve for [X, Y, Z] considering the System of Linear Equations
% We need 4 equations for 4 variables X,Y,Z,XX
S = solve(LHS(1, 1) == (XX * W_invert(1, 1)), ...
LHS(1, 2) == (XX * W_invert(1, 2)), ...
LHS(1, 3) == (XX * W_invert(1, 3)), ...
LHS(2, 2) == (XX * W_invert(2, 2)));
plane = [double(S.X(1)); double(S.Y(1)); double(S.Z(1))];
end
I am implementing Expectation Maximization algorithm in matlab. Algorithm is operating on 214096 x 2 data matrix and While computing probabilities, there is multiplication of ( 214096 x 2 ) * (2 x 2) * ( 2 x 214096 ) matrices, which is resulting in error of out of memory in matlab. Is there a way to fix this problem?
Equation
Matlab Code:
enter image description here D = size(X,2); % dimension
N = size(X,1); % number of samples
K = 4; % number of Gaussian Mixture components ( Also number of clusters )
% Initialization
p = [0.2, 0.3, 0.2, 0.3]; % arbitrary pi, probabilities of clusters, apriori probability of cluster
[idx,mu] = kmeans(X,K); % initial means of the components, theta is mu and variance
% compute the covariance of the components
sigma = zeros(D,D,K);
for k = 1:K
tempmat = X(idx==k,:);
sigma(:,:,k) = cov(tempmat); % Sigma j
sigma_det(k) = det(sigma(:,:,k));
end
% calculate x-mu
for k=1: K
check=length( X(idx == k,1))
for lidx = 1: length( X(idx == k,1))
cidx = find( idx == k) ;
Xmu(cidx(lidx),:) = X(cidx(lidx),:) - mu(k,:); %( x-mu ) calculation on cluster level
end
end
% compute P(Cj|x; theta(t)), and take log to simplified calculation
%Eq 14.14 denominator
denom = 0;
for k=1:K
calc_sigma_1_2 = sigma_det(k)^(-1/2);
calc_x_mu = Xmu(idx == k,:);
calc_sigma_inv = inv(sigma(:,:,k));
calc_x_mu_tran = calc_x_mu.';
factor = calc_sigma_1_2 * exp (-1/2 * calc_x_mu * calc_sigma_inv * calc_x_mu_tran ) * p(k);
denom = denom + factor;
end
for k =1:K
calc_sigma_1_2 = sigma_det(k)^(-1/2);
calc_x_mu = Xmu(idx == k,:);
calc_sigma_inv = inv(sigma(:,:,k));
calc_x_mu_tran = calc_x_mu.';
factor = calc_sigma_1_2 * exp (-1/2 * calc_x_mu_tran * calc_sigma_inv * calc_x_mu ) * p(k);
pdf(k) = factor/denom;
end
%%%% Equation 14.14 ends
It seems that you tried to apply vector based equation by simply substituting vector for matrix, this is not how it works
(x - mu).' * Inv(sigma) * (x-mu)
is supposed to be mahalanobis norm of (x-mu), and you want to obtain this value per each row of matrix X, thus
(X - mu).' * Inv(sigma) =: A <- this is ok, this results in N x d matrix
and now you have to do point-wise multiplication of A with (X - mu), not a dot product, and finally sum over second axis (columns), this way you end up with N element vector, each containing a mahalanobis norm of corresponding row from X.
I am trying to simulate the rotation dynamics of a system. I am testing my code to verify that it's working using simulation, but I never recovered the parameters I pass to the model. In other words, I can't re-estimate the parameters I chose for the model.
I am using MATLAB for that and specifically ode45. Here is my code:
% Load the input-output data
[torque outputs] = DataLogs2();
u = torque;
% using the simulation data
Ixx = 1.00;
Iyy = 2.00;
Izz = 3.00;
x0 = [0; 0; 0];
Ts = .02;
t = 0:Ts:Ts * ( length(u) - 1 );
[ T, x ] = ode45( #(t,x) rotationDyn( t, x, u(1+floor(t/Ts),:), Ixx, Iyy, Izz), t, x0 );
w = x';
N = length(w);
q = 1; % a counter for the A and B matrices
% The Algorithm
for k=1:1:N
w_telda = [ 0 -w(3, k) w(2,k); ...
w(3,k) 0 -w(1,k); ...
-w(2,k) w(1,k) 0 ];
if k == N % to handle the problem of the last iteration
w_dash(:,k) = (-w(:,k))/Ts;
else
w_dash(:,k) = (w(:,k+1)-w(:,k))/Ts;
end
a = kron( w_dash(:,k)', eye(3) ) + kron( w(:,k)', w_telda );
A(q:q+2,:) = a; % a 3N*9 matrix
B(q:q+2,:) = u(k,:)'; % a 3N*1 matrix % u(:,k)
q = q + 3;
end
% Forcing J to be diagonal. This is the case when we consider our quadcopter as two thin uniform
% rods crossed at the origin with a point mass (motor) at the end of each.
A_new = [A(:, 1) A(:, 5) A(:, 9)];
vec_J_diag = A_new\B;
J_diag = diag([vec_J_diag(1), vec_J_diag(2), vec_J_diag(3)])
eigenvalues_J_diag = eig(J_diag)
error = norm(A_new*vec_J_diag - B)
where my dynamic model is defined as:
function [dw, y] = rotationDyn(t, w, tau, Ixx, Iyy, Izz, varargin)
% The output equation
y = [w(1); w(2); w(3)];
% State equation
% dw = (I^-1)*( tau - cross(w, I*w) );
dw = [Ixx^-1 * tau(1) - ((Izz-Iyy)/Ixx)*w(2)*w(3);
Iyy^-1 * tau(2) - ((Ixx-Izz)/Iyy)*w(1)*w(3);
Izz^-1 * tau(3) - ((Iyy-Ixx)/Izz)*w(1)*w(2)];
end
Practically, what this code should do, is to calculate the eigenvalues of the inertia matrix, J, i.e. to recover Ixx, Iyy, and Izz that I passed to the model at the very begining (1, 2 and 3), but all what I get is wrong results.
Is the problem with using ode45?
Well the problem wasn't in the ode45 instruction, the problem is that in system identification one can create an n-1 samples-signal from an n samples-signal, thus the loop has to end at N-1 in the above code.
I am looking for a MATLAB solution to generate the matrix representation of a discrete Radon transform (DRT). That is, given a vectorized version of an MxN image, X, I'd like to generate the matrix R such that R*X(:) is a DRT of the image. In MATLAB, I am expecting it to look something like the following:
>> X = 2D_Image_Of_Size_MxN;
>> R = DRT_Matrix_Of_Size_LPxMN;
>> DRT = reshape( R * X(:), L, P );
I know there are several ways to define a DRT, so I'll just say that I am looking for a normal or standard or not-too-out-of-the-ordinary implmentation.
function [ R rho theta ] = radonmatrix( drho, dtheta, M, N )
% radonmatrix - Discrete Radon Trasnform matrix
%
% SYNOPSIS
% [ R rho theta ] = radonmatrix( drho, dtheta, M, N )
%
% DESCRIPTION
% Returns a matrix representation of a Discrete Radon
% Transform (DRT).
%
% INPUT
% drho Radial spacing the the DRT.
% dtheta Angular spacing of the DRT (rad).
% M Number of rows in the image.
% N Number of columns in the image.
%
% OUTPUT
% R LP x MN DRT matrix. The values of the L and
% P will depend on the radial and angular spacings.
% rho Vector of radial sample locations.
% theta Vector of angular sample locations (rad).
%
% For each angle, we define a set of rays parameterized
% by rho. We then find the pixels on the MxN grid that
% are closest to each line. The elements in R corresponding
% to those pixels are given the value of 1.
% The maximum extent of the region of support. It's for
% rho = 0 and theta = pi/4, the line that runs caddy-corner.
W = sqrt( M^2 + N^2 );
rho = -W/2 : drho : W/2;
theta = 0 : dtheta : 180 - dtheta;
L = length( rho );
P = length( theta );
R = false( L*P, M*N );
% Define a meshgrid w/ (0,0) in the middle that
% we can use a standard coordinate system.
[ mimg nimg ] = imggrid( 1, 1, [ M N ] );
% We loop over each angle and define all of the lines.
% We then just figure out which indices each line goes
% through and put a 1 there.
for ii = 1 : P
phi = theta(ii) * pi/180;
% The equaiton is rho = m * sin(phi) + n * cos(phi).
% We either define a vector for m and solve for n
% or vice versa. We chose which one based on angle
% so that we never g4et close to dividing by zero.
if( phi >= pi/4 && phi <= 3*pi/4 )
t = -W : min( 1/sqrt(2), 1/abs(cot(phi)) ) : +W;
T = length( t );
rhom = repmat( rho(:), 1, T );
tn = repmat( t(:)', L, 1 );
mline = ( rhom - tn * cos(phi) ) ./ sin(phi);
for jj = 1 : L
p = round( tn(jj,:) - min( nimg ) ) + 1;
q = round( mline(jj,:) - min( mimg ) ) + 1;
inds = p >= 1 & p <= N & q >= 1 & q <= M;
R( (ii-1)*L + jj, unique( sub2ind( [ M N ], q(inds), p(inds) ) ) ) = 1;
end
else
t = -W : min( 1/sqrt(2), 1/abs(tan(phi)) ) : +W;
T = length( t );
rhon = repmat( rho(:)', T, 1 );
tm = repmat( t(:), 1, L );
nline = ( rhon - tm * sin(phi) ) ./ cos(phi);
for jj = 1 : L
p = round( nline(:,jj) - min( nimg ) ) + 1;
q = round( tm(:,jj) - min( mimg ) ) + 1;
inds = p >= 1 & p <= N & q >= 1 & q <= M;
R( (ii-1)*L + jj, unique( sub2ind( [ M N ], q(inds), p(inds) ) ) ) = 1;
end
end
end
R = double( sparse( R ) );
return;
Here is the imggrid function used in the above.
function [ m n ] = imggrid( dm, dn, sz )
% imggrid -- Returns rectilinear coordinate vectors
%
% SYNOPSIS
% [ m n ] = imggrid( dm, dn, sz )
%
% DESCRIPTION
% Given the sample spacings and the image size, this
% function returns the row and column coordinate vectors
% for the image. Both vectors are centered about zero.
%
% INPUT
% dm Spacing between rows.
% dn Spacing between columns.
% sz 2x1 vector of the image size: [ Nrows Ncols ].
%
% OUTPUT
% m sz(1) x 1 row coordinate vector.
% n 1 x sz(2) column coordinate vector.
M = sz(1);
N = sz(2);
if( mod( M, 2 ) == 0 )
m = dm * ( ceil( -M/2 ) : floor( M/2 ) - 1 )';
else
m = dm * ( ceil( -M/2 ) : floor( M/2 ) )';
end
if( mod( N, 2 ) == 0 )
n = dn * ( ceil( -N/2 ) : floor( N/2 ) - 1 );
else
n = dn * ( ceil( -N/2 ) : floor( N/2 ) );
end
image = phantom();
projection = radon(image);
R = linsolve(reshape(image,1,[]), reshape(projection,1,[]));