Matrix calculation gets slower after each iteration in matlab - matlab

I have a 1024*1024*51 matrix. I'll do calculations to change some value of the matrix within for loops (change the value of matrix for each iteration). I find that the computing speed gets slower and slower and finally my computer gets into trouble.However the size of the matrix doesn't change. Anyone can shed some light on this problem?
function ActiveContours3D(method,grad,im,mu,nu,lambda1,lambda2,TimeSteps)
epsilon = 10e-10;
tic
fid=fopen('Chr18_z_25of25tiles-C=0_c0_n000.raw','rb','ieee-le');
Xdim = 1024;
Ydim = 1024;
Zdim = 51;
A = fread(fid,[Xdim Ydim*Zdim],'int16');
A = double(A);
size_of_A = size(A)
for(i=1:Zdim)
u0_color(:,:,i) = A(1 : Xdim , (i-1)*Ydim+1 : i*Ydim);
end
fclose(fid)
time = toc
[M,N,P,color] = size(u0_color);
size(u0_color );
u0_color = double(u0_color); % Convert u0_color values to double;
u0 = u0_color(:,:,:,1); % Define the Grayscale volumetric image.
u0_color = uint8(u0_color); % Necessary for color visualization
x = 1:M;
y = 1:N;
z = 1:P;
dx = 1
dy = 1
dz = 1
dim_approx = 2*M*N*P / sqrt(M*N*P);
if(method == 'Explicit')
dt = 0.9 / ((2*mu/(dx^2)) + (2*mu/(dy^2)) + (2*mu/(dz^2))) % 90% CFL
elseif(method == 'Implicit')
dt = (10e7) * 0.9 / ((2*mu/(dx^2)) + (2*mu/(dy^2)) + (2*mu/(dz^2)))
end
[X,Y,Z] = meshgrid(x,y,z);
x0 = (M+1)/2;
y0 = (N+1)/2;
z0 = (P+1)/2;
r0 = min(min(M,N),P)/3;
phi = sqrt((X-x0).^2 + (Y-y0).^2 + (Z-z0).^2) - r0;
phi_visualize = phi; % Use this for visualization in 3D
phi = permute(phi,[2,1,3]); % Use this for computations in 3D
write_to_binary_file(phi_visualize,0); % record initial conditions
tic
for(n=1:TimeSteps)
n
c1 = C1_3d(u0,phi);
c2 = C2_3d(u0,phi);
% x
phi_xp = [phi(2:M,:,:); phi(M,:,:)]; % vertical concatenation
phi_xm = [phi(1,:,:); phi(1:M-1,:,:)]; % (since x values are rows)
% cat(1,A,B) is the same as [A;B]
Dx_m = (phi - phi_xm)/dx; % first derivatives
Dx_p = (phi_xp - phi)/dx;
Dxx = (Dx_p - Dx_m)/dx; % second derivative
% y
phi_yp = [phi(:,2:N,:) phi(:,N,:)]; % horizontal concatenation
phi_ym = [phi(:,1,:) phi(:,1:N-1,:)]; % (since y values are columns)
% cat(2,A,B) is the same as [A,B]
Dy_m = (phi - phi_ym)/dy;
Dy_p = (phi_yp - phi)/dy;
Dyy = (Dy_p - Dy_m)/dy;
% z
phi_zp = cat(3,phi(:,:,2:P),phi(:,:,P));
phi_zm = cat(3,phi(:,:,1) ,phi(:,:,1:P-1));
Dz_m = (phi - phi_zm)/dz;
Dz_p = (phi_zp - phi)/dz;
Dzz = (Dz_p - Dz_m)/dz;
% x,y,z
Dx_0 = (phi_xp - phi_xm) / (2*dx);
Dy_0 = (phi_yp - phi_ym) / (2*dy);
Dz_0 = (phi_zp - phi_zm) / (2*dz);
phi_xp_yp = [phi_xp(:,2:N,:) phi_xp(:,N,:)];
phi_xp_ym = [phi_xp(:,1,:) phi_xp(:,1:N-1,:)];
phi_xm_yp = [phi_xm(:,2:N,:) phi_xm(:,N,:)];
phi_xm_ym = [phi_xm(:,1,:) phi_xm(:,1:N-1,:)];
phi_xp_zp = cat(3,phi_xp(:,:,2:P),phi_xp(:,:,P));
phi_xp_zm = cat(3,phi_xp(:,:,1) ,phi_xp(:,:,1:P-1));
phi_xm_zp = cat(3,phi_xm(:,:,2:P),phi_xm(:,:,P));
phi_xm_zm = cat(3,phi_xm(:,:,1) ,phi_xm(:,:,1:P-1));
phi_yp_zp = cat(3,phi_yp(:,:,2:P),phi_yp(:,:,P));
phi_yp_zm = cat(3,phi_yp(:,:,1) ,phi_yp(:,:,1:P-1));
phi_ym_zp = cat(3,phi_ym(:,:,2:P),phi_ym(:,:,P));
phi_ym_zm = cat(3,phi_ym(:,:,1) ,phi_ym(:,:,1:P-1));
if(grad == 'Dirac')
Grad = DiracDelta(phi); % Dirac delta
%Grad = 1;
elseif(grad == 'Grad ')
Grad = (((Dx_0.^2)+(Dy_0.^2)+(Dz_0.^2)).^(1/2)); % |grad phi|
end
if(method == 'Explicit')
% CURVATURE: *mu*k|grad phi|* (central differences):
K = zeros(M,N,P);
Dxy = (phi_xp_yp - phi_xp_ym - phi_xm_yp + phi_xm_ym) / (4*dx*dy);
Dxz = (phi_xp_zp - phi_xp_zm - phi_xm_zp + phi_xm_zm) / (4*dx*dz);
Dyz = (phi_yp_zp - phi_yp_zm - phi_ym_zp + phi_ym_zm) / (4*dy*dz);
K = ( (Dx_0.^2).*Dyy - 2*Dx_0.*Dy_0.*Dxy + (Dy_0.^2).*Dxx ...
+ (Dx_0.^2).*Dzz - 2*Dx_0.*Dz_0.*Dxz + (Dz_0.^2).*Dxx ...
+ (Dy_0.^2).*Dzz - 2*Dy_0.*Dz_0.*Dyz + (Dz_0.^2).*Dyy) ./ ((Dx_0.^2 + Dy_0.^2 + Dz_0.^2).^(3/2) + epsilon);
phi_temp = phi + dt * Grad .* ( mu.*K + lambda1*(u0 - c1).^2 - lambda2*(u0 - c2).^2 );
elseif(method == 'Implicit')
C1x = 1 ./ sqrt(Dx_p.^2 + Dy_0.^2 + Dz_0.^2 + (10e-7)^2);
C2x = 1 ./ sqrt(Dx_m.^2 + Dy_0.^2 + Dz_0.^2 + (10e-7)^2);
C3y = 1 ./ sqrt(Dx_0.^2 + Dy_p.^2 + Dz_0.^2 + (10e-7)^2);
C4y = 1 ./ sqrt(Dx_0.^2 + Dy_m.^2 + Dz_0.^2 + (10e-7)^2);
C5z = 1 ./ sqrt(Dx_0.^2 + Dy_0.^2 + Dz_p.^2 + (10e-7)^2);
C6z = 1 ./ sqrt(Dx_0.^2 + Dy_0.^2 + Dz_m.^2 + (10e-7)^2);
% m = (dt/(dx*dy)) * Grad .* mu; % 2D
m = (dt/(dx*dy)) * Grad .* mu;
C = 1 + m.*(C1x + C2x + C3y + C4y + C5z + C6z);
C1x_2x = C1x.*phi_xp + C2x.*phi_xm;
C3y_4y = C3y.*phi_yp + C4y.*phi_ym;
C5z_6z = C5z.*phi_zp + C6z.*phi_zm;
phi_temp = (1 ./ C) .* ( phi + m.*(C1x_2x+C3y_4y) + (dt*Grad).*(lambda1*(u0 - c1).^2) - (dt*Grad).*(lambda2*(u0 - c2).^2) );
end
phi = phi_temp;
phi_visualize = permute(phi,[2,1,3]);
write_to_binary_file(phi_visualize,n); % record
end
time = toc
n = n
T = dt*n
clear
clear all

In general Matlab keeps track of all the variables in the form of matrix. When you work with lot of variables with many dimensions the RAM memory will be allocated for storing this variable. Hence on working with lots of variables that is gonna run for multiple iterations it is better to clear the variable from the memory. To do so use the command
clear variable_name_1, variable_name_2,... variable_name_3;
Although keeping all the variables keeps the code to look organised, however when you face such issues try clearing the unwanted variables.
Check this link to use clear command in detail: http://www.mathworks.in/help/matlab/ref/clear.html

Related

Euclidian distances in speaker recognition system

I'm new in Matlab and now I have a problem for the implementation of a simple speaker recognition system using PNCC and MFFC.
My problem is on matrix dimension in fact, when I run my program, it give me this error:
Matrix dimensions must agree.
Error in disteu (line 43)
d(n,:) = sum((x(:, n+copies) - y) .^2, 1);
Error in test (line 22)
d = disteu(v, code{l});
Error in main (line 4)
test('C:\Users\Antonio\Documents\MATLAB\test',5, code);
Just for the sake of clarity I have attached my code.
Could anyone help me please?
function d = disteu(x, y)
% DISTEU Pairwise Euclidean distances between columns of two matrices
%
% Input:
% x, y: Two matrices whose each column is an a vector data.
%
% Output:
% d: Element d(i,j) will be the Euclidean distance between two
% column vectors X(:,i) and Y(:,j)
%
% Note:
% The Euclidean distance D between two vectors X and Y is:
% D = sum((x-y).^2).^0.5
% D = sum((x-y).^2).^0.5
[M, N] = size(x);
[M2, P] = size(y);
if (M ~= M2)
y=padarray(y,0,0,'post');
x=padarray(x,21,0,'post');
[M, N] = size(x)
[M2, P] = size(y)
y=padarray(y,0,0,'post');
[M2, P] = size(y)
end
%error('Matrix dimensions do not match.')
d = zeros(N, P);
if (N < P)
copies = zeros(1,P);
for n = 1:N
d(n,:) = sum((x(:, n+copies) - y) .^2, 1);
end
else
copies = zeros(1,N);
for p = 1:P
d(:,p) = sum((x - y(:, p+copies)) .^2, 1)';
end
end
d = d.^0.5;
function [aadDCT] = PNCC(rawdata, fsamp)
ad_x = rawdata;
%addpath voicebox/; % With Spectral Subtraction - default parameters
%ad_x = specsub(rawdata, fsamp);
dLamda_L = 0.999;
dLamda_S = 0.999;
dSampRate = fsamp;
dLowFreq = 200;% Changed to 40 from 200 as low freq is 40 in gabor as well
dHighFreq = dSampRate / 2;
dPowerCoeff = 1 / 15;
iFiltType = 1;
dFactor = 2.0;
dGammaThreshold = 0.005;
iM = 0; % Changed from 2 to 0 as number of frames coming out to be different due to queue
iN = 4;
iSMType = 0;
dLamda = 0.999;
dLamda2 = 0.5;
dDelta1 = 1;
dLamda3 = 0.85;
dDelta2 = 0.2;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Flags
%
bPreem = 1; % pre-emphasis flag
bSSF = 1;
bPowerLaw = 1;
bDisplay = 0;
dFrameLen = 0.025; % 25.6 ms window length, which is the default setting in CMU Sphinx
dFramePeriod = 0.010; % 10 ms frame period
iPowerFactor = 1;
global iNumFilts;
iNumFilts = 40;
if iNumFilts<30
iFFTSize = 512;
else
iFFTSize = 1024;
end
% For derivatives
deltawindow = 2; % to calculate 1st derivative
accwindow = 2; % to calculate 2nd derivative
% numcoeffs = 13; % number of cepstral coefficients to be used
numcoeffs = 13;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Flags
%
%
% Array Queue Ring-buffer
%
global Queue_aad_P;
global Queue_iHead;
global Queue_iTail;
global Queue_iWindow;
global Queue_iNumElem;
Queue_iWindow = 2 * iM + 1;
Queue_aad_P = zeros(Queue_iWindow, iNumFilts);
Queue_iHead = 0;
Queue_iTail = 0;
Queue_iNumElem = 0;
iFL = floor(dFrameLen * dSampRate);
iFP = floor(dFramePeriod * dSampRate);
iNumFrames = floor((length(ad_x) - iFL) / iFP) + 1;
iSpeechLen = length(ad_x);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Pre-emphasis using H(z) = 1 - 0.97 z ^ -1
%
if (bPreem == 1)
ad_x = filter([1 -0.97], 1, double(ad_x));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Obtaning the gammatone coefficient.
%
% Based on M. Snelly's auditory toolbox.
% In actual C-implementation, we just use a table
%
bGamma = 1;
[wts,binfrqs] = fft2melmx(iFFTSize, dSampRate, iNumFilts, 1, dLowFreq, dHighFreq, iFiltType);
wts = wts';
wts(size(wts, 1) / 2 + 1 : size(wts, 1), : ) = [];
aad_H = wts;
i_FI = 0;
i_FI_Out = 0;
if bSSF == 1
adSumPower = zeros(1, iNumFrames - 2 * iM);
else
adSumPower = zeros(1, iNumFrames);
end
%dLamda_L = 0.998;
aad_P = zeros(iNumFrames, iNumFilts);
aad_P_Out = zeros(iNumFrames - 2 * iM, iNumFilts);
ad_Q = zeros(1, iNumFilts);
ad_Q_Out = zeros(1, iNumFilts);
ad_QMVAvg = zeros(1, iNumFilts);
ad_w = zeros(1, iNumFilts);
ad_w_sm = zeros(1, iNumFilts);
ad_QMVAvg_LA = zeros(1, iNumFilts);
MEAN_POWER = 1e10;
dMean = 5.8471e+08;
dPeak = 2.7873e+09 / 15.6250;
% (1.7839e8, 2.0517e8, 2.4120e8, 2.9715e8, 3.9795e8) 95, 96, 97, 98, 99
% percentile from WSJ-si84
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dPeakVal = 4e+07;% % 4.0638e+07 --> Mean from WSJ0-si84 (Important!!!)
%%%%%%%%%%%
dMean = dPeakVal;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Obtaining the short-time Power P(i, j)
%
for m = 0 : iFP : iSpeechLen - iFL
ad_x_st = ad_x(m + 1 : m + iFL) .* hamming(iFL);
adSpec = fft(ad_x_st, iFFTSize);
ad_X = abs(adSpec(1: iFFTSize / 2));
aadX(:, i_FI + 1) = ad_X;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Calculating the Power P(i, j)
%
for j = 1 : iNumFilts
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Squared integration
%
if iFiltType == 2
aad_P(i_FI + 1, j) = sum((ad_X .* aad_H(:, j)) .^ 2);
else
aad_P(i_FI + 1, j) = sum((ad_X .^ 2 .* aad_H(:, j)));
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Calculating the Power P(i, j)
%
dSumPower = sum(aad_P(i_FI + 1, : ));
if bSSF == 1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Ring buffer (using a Queue)
%
if (i_FI >= 2 * iM + 1)
Queue_poll();
end
Queue_offer(aad_P(i_FI + 1, :));
ad_Q = Queue_avg();
if (i_FI == 2 * iM)
ad_QMVAvg = ad_Q.^ (1 / 15);
ad_PBias = (ad_Q) * 0.9;
end
if (i_FI >= 2 * iM)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Bias Update
%
for i = 1 : iNumFilts,
if (ad_Q(i) > ad_PBias(i))
ad_PBias(i) = dLamda * ad_PBias(i) + (1 - dLamda) * ad_Q(i);
else
ad_PBias(i) = dLamda2 * ad_PBias(i) + (1 - dLamda2) * ad_Q(i);
end
end
for i = 1 : iNumFilts,
ad_Q_Out(i) = max(ad_Q(i) - ad_PBias(i), 0) ;
if (i_FI == 2 * iM)
ad_QMVAvg2(i) = 0.9 * ad_Q_Out(i);
ad_QMVAvg3(i) = ad_Q_Out(i);
ad_QMVPeak(i) = ad_Q_Out(i);
end
if (ad_Q_Out(i) > ad_QMVAvg2(i))
ad_QMVAvg2(i) = dLamda * ad_QMVAvg2(i) + (1 - dLamda) * ad_Q_Out(i);
else
ad_QMVAvg2(i) = dLamda2 * ad_QMVAvg2(i) + (1 - dLamda2) * ad_Q_Out(i);
end
dOrg = ad_Q_Out(i);
ad_QMVAvg3(i) = dLamda3 * ad_QMVAvg3(i);
if (ad_Q(i) < dFactor * ad_PBias(i))
ad_Q_Out(i) = ad_QMVAvg2(i);
else
if (ad_Q_Out(i) <= dDelta1 * ad_QMVAvg3(i))
ad_Q_Out(i) = dDelta2 * ad_QMVAvg3(i);
end
end
ad_QMVAvg3(i) = max(ad_QMVAvg3(i), dOrg);
ad_Q_Out(i) = max(ad_Q_Out(i), ad_QMVAvg2(i));
end
ad_w = ad_Q_Out ./ max(ad_Q, eps);
for i = 1 : iNumFilts,
if iSMType == 0
ad_w_sm(i) = mean(ad_w(max(i - iN, 1) : min(i + iN ,iNumFilts)));
elseif iSMType == 1
ad_w_sm(i) = exp(mean(log(ad_w(max(i - iN, 1) : min(i + iN ,iNumFilts)))));
elseif iSMType == 2
ad_w_sm(i) = mean((ad_w(max(i - iN, 1) : min(i + iN ,iNumFilts))).^(1/15))^15;
elseif iSMType == 3
ad_w_sm(i) = (mean( (ad_w(max(i - iN, 1) : min(i + iN ,iNumFilts))).^15 )) ^ (1 / 15);
end
end
aad_P_Out(i_FI_Out + 1, :) = ad_w_sm .* aad_P(i_FI - iM + 1, :);
adSumPower(i_FI_Out + 1) = sum(aad_P_Out(i_FI_Out + 1, :));
if adSumPower(i_FI_Out + 1) > dMean
dMean = dLamda_S * dMean + (1 - dLamda_S) * adSumPower(i_FI_Out + 1);
else
dMean = dLamda_L * dMean + (1 - dLamda_L) * adSumPower(i_FI_Out + 1);
end
aad_P_Out(i_FI_Out + 1, :) = aad_P_Out(i_FI_Out + 1, :) / (dMean) * MEAN_POWER;
i_FI_Out = i_FI_Out + 1;
end
else % if not SSF
adSumPower(i_FI + 1) = sum(aad_P(i_FI + 1, :));
if adSumPower(i_FI_Out + 1) > dMean
dMean = dLamda_S * dMean + (1 - dLamda_S) * adSumPower(i_FI_Out + 1);
else
dMean = dLamda_L * dMean + (1 - dLamda_L) * adSumPower(i_FI_Out + 1);
end
aad_P_Out(i_FI + 1, :) = aad_P(i_FI + 1, :) / (dMean) * MEAN_POWER;
end
i_FI = i_FI + 1;
end
%adSorted = sort(adSumPower);
%dMaxPower = adSorted(round(0.98 * length(adSumPower)));
%aad_P_Out = aad_P_Out / dMaxPower * 1e10;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Apply the nonlinearity
%
%dPowerCoeff
if bPowerLaw == 1
aadSpec = aad_P_Out .^ dPowerCoeff;
else
aadSpec = log(aad_P_Out + eps);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% DCT
%
aadDCT = dct(aadSpec')';
%aadDCT(:, numcoeffs+1:iNumFilts) = [];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% MVN
%
% for i = 1 : numcoeffs
% aadDCT( :, i ) = (aadDCT( : , i ) - mean(aadDCT( : , i)))/std(aadDCT(:,i));
% end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Temporal Derivatives
% calculate 1st derivative (velocity)
dt1 = deltacc(aadDCT', deltawindow);
% calculate 2nd derivative (acceleration)
dt2 = deltacc(dt1, accwindow);
% append dt1 and dt2 to mfcco
aadDCT = [aadDCT'; dt2];
% aadDCT = [aadDCT'; dt2];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Display
%
if bDisplay == 1
figure
aadSpec = idct(aadDCT', iNumFilts);
imagesc(aadSpec); axis xy;
end
aadDCT = aadDCT';
%{
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Writing the feature in Sphinx format
%
[iM, iN] = size(aadDCT);
iNumData = iM * iN;
fid = fopen(szOutFeatFileName, 'wb');
fwrite(fid, iNumData, 'int32');
iCount = fwrite(fid, aadDCT(:), 'float32');
fclose(fid);
%}
end
function dt = deltacc(input, winlen)
% calculates derivatives of a matrix, whose columns are feature vectors
tmp = 0;
for cnt = 1 : winlen
tmp = tmp + cnt*cnt;
end
nrm = 1 / (2*tmp);
dt = zeros(size(input));
rows = size(input,1);
cols = size(input,2);
for col = 1 : cols
for cnt = 1 : winlen
inx1 = col - cnt; inx2 = col + cnt;
if inx1 < 1; inx1 = 1; end
if inx2 > cols; inx2 = cols; end
dt(:, col) = dt(:, col) + (input(:, inx2) - input(:, inx1)) * cnt;
end
end
dt = dt * nrm;
end
function [] = Queue_offer(ad_x)
global Queue_aad_P;
global Queue_iHead;
global Queue_iTail;
global Queue_iWindow;
global Queue_iNumElem;
Queue_aad_P(Queue_iTail + 1, :) = ad_x;
Queue_iTail = mod(Queue_iTail + 1, Queue_iWindow);
Queue_iNumElem = Queue_iNumElem + 1;
if Queue_iNumElem > Queue_iWindow
error ('Queue overflow');
end
end
function [ad_x] = Queue_poll()
global Queue_aad_P;
global Queue_iHead;
global Queue_iTail;
global Queue_iWindow;
global Queue_iNumElem;
if Queue_iNumElem <= 0
error ('No elements');
end
ad_x = Queue_aad_P(Queue_iHead + 1, :);
Queue_iHead = mod(Queue_iHead + 1, Queue_iWindow);
Queue_iNumElem = Queue_iNumElem - 1;
end
function[adMean] = Queue_avg()
global Queue_aad_P;
global Queue_iHead;
global Queue_iTail;
global Queue_iWindow;
global Queue_iNumElem;
global iNumFilts;
adMean = zeros(1, iNumFilts); % Changed from 40 (number of filter banks)
iPos = Queue_iHead;
for i = 1 : Queue_iNumElem
adMean = adMean + Queue_aad_P(iPos + 1 ,: );
iPos = mod(iPos + 1, Queue_iWindow);
end
adMean = adMean / Queue_iNumElem;
end
function test(testdir, n, code)
for k = 1:n % read test sound file of each speaker
file = sprintf('%ss%d.wav', testdir, k);
[s, fs] = audioread(file);
%x = s + 0.01*randn(length(s),1); %AWGN Noise
%[SNR1] = snr(s);
%[SNR2] = snr(x) ;
v = PNCC(s, fs); % Compute MFCC's
distmin = inf;
k1 = 0;
for l = 1:length(code) % each trained codebook, compute distortion
d = disteu(v, code{l});
dist = sum(min(d,[],2)) / size(d,1);
if dist < distmin
distmin = dist;
k1 = l;
end
end
msg = sprintf('speaker%d -->> s%d', k, k1);
disp(msg);
end
function r = vqlbg(d,k)
%
% Inputs: d contains training data vectors (one per column)
% k is number of centroids required
e = .01;
r = mean(d, 2);
dpr = 10000;
for i = 1:log2(k)
r = [r*(1+e), r*(1-e)];
while (1 == 1)
z = interdists(d, r);
[m,ind] = min(z, [], 2);
t = 0;
for j = 1:2^i
r(:, j) = mean(d(:, find(ind == j)), 2);
x = interdists(d(:, find(ind == j)), r(:, j));
for q = 1:length(x)
t = t + x(q);
end
end
if (((dpr - t)/t) < e)
break;
else
dpr = t;
end
end
end %Output: r contains the result VQ codebook (k columns, one for each centroids)

Sequential Quadratic Programming Matlab Implementation

I have a problem with my MATLAB code that solves a nonlinear quadratic problem with SQP algorithm (Sequential quadratic programming) but in the "QP-SUB PROBLEM" section of the code that i have formulated analytically a "num2str"error appears and honestly, i don't know how to fix that and also have to tell you that this method uses KT conditions for a better solution .
In every section of the code i write a comment for better understanding and function with constraints can be found in the code below :
% Maximize f(x1,x2) = x1^4 -2x1^2x2 +x1^2 +x1x2^2 -2x1 +4
%
% h1(x1,x2) = x1^2 + x2^2 -2 = 0
% g1(x1,x2) = 0.25x1^2 +0.75x2^2 -1 <=0
% 0 <= x1 <= 4; 0 <= x2 <= 4
%
%--------------------------------------------------------
% The KT conditions for the QP subproblem
% are applied analytically
% There are two cases for a single inequality constraint
% Case (a) : beta = 0 g < 0
% Case (b) : beta ~= 0, g = 0
% The best solution is used
% --------------------------------------------------------
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% management functions
clear % clear all variable/information in the workspace - use CAUTION
clear global % again use caution - clears global information
clc % position the cursor at the top of the screen
close % closes the figure window
format compact % avoid skipping a line when writing to the command window
warning off %#ok<WNOFF> % don't report any warnings like divide by zero etc.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% DATA ---------------------
%%% starting design
xb(1) = 3; xb(2) = 2;
it = 10; % number of iterations
%%% plot range for delx1 : -3 , +3
dx1L = -3; dx1U = +3;
dx2L = -3; dx2U = +3;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Define functions
syms x1 x2 f g h
syms gradf1 gradf2 gradh1 gradh2 gradg1 gradg2
f = x1^4 - 2*x1*x1*x2 + x1*x1 + x1*x2*x2 - 2*x1 + 4;
h = x1*x1 + x2*x2 - 2;
g = 0.25*x1*x1 +0.75*x2*x2 -1;
%%% the gradient functions
gradf1 = diff(f,x1);
gradf2 = diff(f,x2);
% the hessian
hess = [diff(gradf1,x1), diff(gradf1,x2); diff(gradf2,x1), diff(gradf2,x2)];
% gradient of the constraints
gradh1 = diff(h,x1);
gradh2 = diff(h,x2);
gradg1 = diff(g,x1);
gradg2 = diff(g,x2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% graphical/symbolic solution for SLP
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('***********************')
fprintf('\nSQP - Example 7.1')
fprintf('\n*********************\n')
for i = 1:it
%figure;
f1 = subs(f,{x1,x2},{xb(1),xb(2)});
g1 = subs(g,{x1,x2},{xb(1),xb(2)});
h1 = subs(h,{x1,x2},{xb(1),xb(2)});
%%% Print Information
fprintf('\n******************************')
fprintf('\nIteration : '),disp(i)
fprintf('******************************\n')
fprintf('Linearized about [x1, x2] : '),disp([xb(1) xb(2)])
fprintf('Objective function value f(x1,x2) : '),disp(f1);
fprintf('Equality constraint value value h(x1,x2) : '),disp(h1);
fprintf('Inequality constraint value value g(x1,x2) : '),disp(g1);
%fprintf('\nsolution for [delx1 delx2] : '),disp(sol')
% hold on
% calculate the value of the gradients
% f1 = subs(f,{x1,x2},{xb(1),xb(2)});
% g1 = subs(g,{x1,x2},{xb(1),xb(2)});
% h1 = subs(h,{x1,x2},{xb(1),xb(2)});
fprintf('\n-----------------------')
fprintf('\nQP - SUB PROBLEM')
fprintf('\n---------------------\n')
gf1 = double(subs(gradf1,{x1,x2},{xb(1),xb(2)}));
gf2 = double(subs(gradf2,{x1,x2},{xb(1),xb(2)}));
hess1 = double(subs(hess,{x1,x2},{xb(1),xb(2)}));
gh1 = double(subs(gradh1,{x1,x2},{xb(1),xb(2)}));
gh2 = double(subs(gradh2,{x1,x2},{xb(1),xb(2)}));
gg1 = double(subs(gradg1,{x1,x2},{xb(1),xb(2)}));
gg2 = double(subs(gradg2,{x1,x2},{xb(1),xb(2)}));
% the QP subproblem
syms dx1 dx2 % change in design
fquad = f1 + [gf1 gf2]*[dx1; dx2] + 0.5*[dx1 dx2]*hess1*[dx1 ;dx2];
hlin = h1 + [gh1 gh2]*[dx1; dx2];
glin = g1 + [gg1 gg2]*[dx1; dx2];
Fquadstr = strcat(num2str(f1),' + ',num2str(gf1), ...
'*','dx1',' + ',num2str(gf2),' * ','dx2', ...
' + 0.5*',num2str(hess1(1,1)),' * dx1^2', ...
' +',num2str(hess1(1,2)),' * dx1*dx2', ...
' + 0.5*',num2str(hess1(2,2)),' * dx2^2');
hlinstr = strcat(num2str(h1),' + ',num2str(gh1), ...
'*','dx1',' + ',num2str(gh2),' * ','dx2');
glinstr = strcat(num2str(g1),' + ',num2str(gg1), ...
'*','dx1',' + ',num2str(gg2),' * ','dx2');
fprintf('Quadratic Objective function f(x1,x2): \n'),disp(Fquadstr);
fprintf('Linearized equality h(x1,x2): '),disp(hlinstr);
fprintf('Linearized inequality g(x1,x2): '),disp(glinstr);
fprintf('\n')
% define Lagrangian for the QP problem
syms lamda beta
F = fquad + lamda*hlin + beta*glin;
fprintf('Case a: beta = 0\n');
Fnobeta = fquad + lamda*hlin;
%%% initialize best solution
dx1best = 0;
dx2best = 0;
Fbbest = 0;
%%%%%%%%%%%%%%%%%%%%%%%
%%% solve case (a) %%%
%%%%%%%%%%%%%%%%%%%%%%%
xcasea = solve(diff(Fnobeta,dx1),diff(Fnobeta,dx2),hlin);
sola = [double(xcasea.dx1) double(xcasea.dx2) double(xcasea.lamda)];
dx1a = double(xcasea.dx1);
dx2a = double(xcasea.dx2);
lamdaa = double(xcasea.lamda);
hlina = double(subs(hlin,{dx1,dx2},{dx1a,dx2a}));
glina = double(subs(glin,{dx1,dx2},{dx1a,dx2a}));
Fa = double(subs(Fnobeta,{dx1,dx2,lamda},{dx1a,dx2a,lamdaa}));
%%% results for case (a)
x1a = dx1a + xb(1);
x2a = dx2a + xb(2);
fv = double(subs(f,{x1,x2},{x1a,x2a}));
hv = double(subs(h,{x1,x2},{x1a,x2a}));
gv = double(subs(g,{x1,x2},{x1a,x2a}));
fprintf('Change in design vector: '),disp([dx1a dx2a]);
fprintf('The linearized quality constraint: '),disp(hlina);
fprintf('The linearized inequality constraint: '),disp(glina);
fprintf('New design vector: '),disp([x1a x2a]);
fprintf('The objective function: '),disp(fv);
fprintf('The equality constraint: '),disp(hv);
fprintf('The inequality constraint: '),disp(gv);
if (glina <= 0)
xb(1) = xb(1) + dx1a;
xb(2) = xb(2) + dx2a;
fbest = Fa;
dx1best = dx1a;
dx2best = dx2a;
end
%%%%%%%%%%%%%%%%%%%%%%%
%%% solve case (b) %%%
%%%%%%%%%%%%%%%%%%%%%%%
fprintf('\n Case b: g = 0\n');
xcaseb = solve(diff(F,dx1),diff(F,dx2),hlin,glin);
solb = [double(xcaseb.dx1) double(xcaseb.dx2) double(xcaseb.lamda) double(xcaseb.beta)];
dx1b = double(xcaseb.dx1);
dx2b = double(xcaseb.dx2);
betab = double(xcaseb.beta);
lamdab = double(xcaseb.lamda);
hlinb = double(subs(hlin,{dx1,dx2},{dx1b,dx2b}));
glinb = double(subs(glin,{dx1,dx2},{dx1b,dx2b}));
Fb = double(subs(F,{dx1,dx2,lamda,beta},{dx1b,dx2b,lamdab,betab}));
x1b = dx1b + xb(1);
x2b = dx2b + xb(2);
fv = double(subs(f,{x1,x2},{x1b,x2b}));
hv = double(subs(h,{x1,x2},{x1b,x2b}));
gv = double(subs(g,{x1,x2},{x1b,x2b}));
fprintf('Change in design vector: '),disp([dx1b dx2b]);
fprintf('The linearized quality constraint: '),disp(hlinb);
fprintf('The linearized inequality constraint: '),disp(glinb);
fprintf('New design vector: '),disp([x1b x2b]);
fprintf('The objective function: '),disp(fv);
fprintf('The equality constraint: '),disp(hv);
fprintf('The inequality constraint: '),disp(gv);
fprintf('Multiplier beta: '),disp(betab);
fprintf('Multiplier lamda: '),disp(lamdab);
if (betab > 0) & (Fb <= fbest)
xb(1) = x1b;
xb(2) = x2b;
dx1best = dx1b;
dx2best = dx2b;
end
%%% stopping criteria
if ([dx1best dx2best]*[dx1best dx2best]') <= 1.0e-08
fprintf('\n&&&&&&&&&&&&&&&&&&&&&&&&&&&&&')
fprintf('\nStopped: Design Not Changing')
fprintf('\n&&&&&&&&&&&&&&&&&&&&&&&&&&&&&\n\n')
break;
elseif i == it
fprintf('\n&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&')
fprintf('\nStpped: Number of iterations at maximum')
fprintf('\n&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&\n\n')
break;
end
end
f1, g1, h1 are still syms variable type.
Change them to numeric type using the function double() before applying
the function num2str()
You have already applied double() to the variables
gf1, gf2, gh1, gh2,gg1, gg2 and hess1
right above, no need to touch them
Here is the section you should replace
Fquadstr = strcat(num2str(f1),' + ',num2str(gf1), ...
'*','dx1',' + ',num2str(gf2),' * ','dx2', ...
' + 0.5*',num2str(hess1(1,1)),' * dx1^2', ...
' +',num2str(hess1(1,2)),' * dx1*dx2', ...
' + 0.5*',num2str(hess1(2,2)),' * dx2^2');
hlinstr = strcat(num2str(h1),' + ',num2str(gh1), ...
'*','dx1',' + ',num2str(gh2),' * ','dx2');
glinstr = strcat(num2str(g1),' + ',num2str(gg1), ...
'*','dx1',' + ',num2str(gg2),' * ','dx2');
by this
% apply double() to f1
Fquadstr = strcat(num2str(double(f1)),' + ',num2str(gf1), ...
'*','dx1',' + ',num2str(gf2),' * ','dx2', ...
' + 0.5*',num2str(hess1(1,1)),' * dx1^2', ...
' +',num2str(hess1(1,2)),' * dx1*dx2', ...
' + 0.5*',num2str(hess1(2,2)),' * dx2^2');
% apply double() to h1
hlinstr = strcat(num2str(double(h1)),' + ',num2str(gh1), ...
'*','dx1',' + ',num2str(gh2),' * ','dx2');
% apply double() to g1
glinstr = strcat(num2str(double(g1)),' + ',num2str(gg1), ...
'*','dx1',' + ',num2str(gg2),' * ','dx2');

Error state kalman filter estimates wrong values

I've implemented this error state kalman filter, which has IMU data as input (220Hz) and it corrects the prediction with UWB measurements (50 Hz).
I want to estimate the pose of a quadrotor.
The code is:
function [x_hat,bound_p] = ESKF(d_meas,p_am,clockUWB,dt,u)
% u = IMU inputs
% d_meas = UWB measure
% p_am = anchor coordinate. There are 4 anchors that send the measurement one at a time cyclically
g = [0 0 9.81]';
am = u(1:3); % from acc
wm = u(4:6); % from gyro
persistent P Q R I dx_hat x_n p q v ba bw Fw
if isempty(P)
sig_an = 0.05; % [m/s^2]
sig_wn = 0.01; % [rad/s]
sig_wbn = 0.0001; % [rad/s*sqrt(s)]
sig_abn = 0.0001; % [m/(s^2*sqrt(s)]
sig_uwb = 0.0214; % [m]
Q_an = sig_an*sig_an*eye(3); % [m^2/s^2]
Q_wn = sig_wn*sig_wn*eye(3); % [rad^2]
Q_abn = sig_abn*sig_abn*eye(3); % [m^2/s^4]
Q_wbn = sig_wbn*sig_wbn*eye(3); % [rad^2/s^2]
clockUWB = 0;
dx_hat = zeros(15,1); % error state
x_n = [5 4 5 1 0 0 1 0 0 0 0 0 0 0 0 0]'; % initial state
P = eye(15,15);
Fw = [zeros(3,12);eye(12,12)];
Q = blkdiag(Q_an,Q_wn,Q_wbn,Q_abn);
R = sig_uwb*sig_uwb;
I = eye(length(dx_hat));
p = x_n(1:3);
v = x_n(4:6);
q = x_n(7:10);
bw = x_n(11:13);
ba = x_n(14:16);
end
%% NOMINAL STATE
if ((wm - bw) == [0 0 0]')
qw = [1 0 0 0]';
else
nw = norm(wm - bw);
qw = [cos(nw*dt/2); ((wm - bw)/nw)*sin(nw*dt/2)];
end
R_ui = quat2rotm(q');
% PREDICTION
p = p + v*dt + 1/2*(R_ui*(am - ba) + g)*dt*dt;
v = v + (R_ui*(am - ba) + g)*dt;
q = quatmultiply(q',qw')';
q = q/norm(q);
% bw = bw;
% ba = ba;
%% ERROR STATE
% delta_p = delta_p + delta_v*dt;
% delta_v = delta_v + (-R_ui*skew(am - ba)*delta_th - R_ui*delta_ba)*dt + an);
% delta_th = R_ui'*delta_th - delta_bw*dt + wn;
% delta_bw = delta_bw + wbn*ones(3,1);
% delta_ba = delta_ba + abn*ones(3,1);
F = [ eye(3), eye(3)*dt, zeros(3,3), zeros(3,3), zeros(3,3);
zeros(3,3), eye(3), -R_ui*skew(am-ba)*dt, zeros(3,3), -R_ui*dt;
zeros(3,3), zeros(3,3), R_ui', -eye(3)*dt, zeros(3,3);
zeros(3,3), zeros(3,3), zeros(3,3), eye(3), zeros(3,3);
zeros(3,3), zeros(3,3), zeros(3,3), zeros(3,3), eye(3)];
%PREDCTION
dx_hat = F*dx_hat;
P = F * P * F' + Fw * Q * Fw';
%% UPDATE
% Only when measures arrive
if (clockUWB == 1)
h = p - p_am;
d_am = norm(h);
H = [ (1/(2*d_am))*2*h'*eye(3), zeros(1,3), zeros(1,3), zeros(1,3), zeros(1,3)];
K = P * H' * inv(H * P * H' + R);
dx_hat = K * (d_meas - d_am);
delta_x = [dx_hat(1:6);[1,1/2*dx_hat(7:9)']';dx_hat(10:15)];
P = (I - K * H) * P * (I - K * H)' + K * R * K';
%% ERROR INJECTION
p = p + delta_x(1:3);
v = v + delta_x(4:6);
q = quatmultiply(q',delta_x(7:10)')';
q = q/norm(q);
bw = bw + delta_x(11:13);
ba = ba + delta_x(14:16);
sig_p = 3*[sqrt(P(1,1));sqrt(P(2,2));sqrt(P(2,2))];
bound_p = [p(1) + sig_p(1),p(1) - sig_p(1),p(2) + sig_p(2),p(2) - sig_p(2),p(3) + sig_p(3),p(3) - sig_p(3)];
x_hat = [p;v;q;bw;ba];
yaw_err = abs(dx_hat(9));
yaw_err_old = yaw_err;
%% RESET ERROR
G = blkdiag(eye(6),eye(3) - skew(1/2*dx_hat(7:9)),eye(6));
delta_x = zeros(16,1);
dx_hat = zeros(15,1);
P = G * P * G';
else
x_hat = [p;v;q;bw;ba];
delta_x = zeros(16,1);
sig_p = 3*[sqrt(P(1,1));sqrt(P(2,2));sqrt(P(2,2))];
bound_p = [p(1) + sig_p(1),p(1) - sig_p(1),p(2) + sig_p(2),p(2) - sig_p(2),p(3) + sig_p(3),p(3) - sig_p(3)];
end
end
It estimates position perfectly, and also velocity is nice.
The problem is that it estimates roll, pitch and yaw (which I derive from the quaternion thanks to quat2eul function) very badly and some bias are totally wrong.
Can someone tell me where the code is wrong?
Thanks
This is the main simulink model
In the green block there is the function of the filter.
In order to simulate a trajectory and UWB measurements in the UWB block there is this script:
function [xt,yt,zt,d_meas,p_am] = fcn(t,clock)
sig_uwb = 0.0214;
dn = normrnd(0,sig_uwb);
persistent k i
if isempty(k)
k = 0;
clock = 0;
i = 0;
end
x = 3*cos(1/3*t) + 3;
y = 3*sin(1/3*t) + 3;
z = 5;
a1 = [-4.12,-3.67,2.72]; % anchors coordinates
a2 = [2.45,-2.70,0.063];
a3 = [-2.43,3.07,0.075];
a4 = [3.65,2.42,2.62];
d1 = norm(a1 - [x,y,z]);
d2 = norm(a2 - [x,y,z]);
d3 = norm(a3 - [x,y,z]);
d4 = norm(a4 - [x,y,z]);
A = [a1,d1;a2,d2;a3,d3;a4,d4];
if (clock == 1)
k = k + 1;
i = mod(k,4) + 1;
d_meas = A(i,4) + dn;
p_am = A(i,(1:3))';
else
d_meas = 0;
p_am = zeros(3,1);
end
xt = x;
yt = y;
zt = z;
So the drone simulates a circular trajectory with radius equals to 3.
The IMU block contains just 2 vector:
am = [0 -1/3 -9.81] and wm = [0 0 1/3].
The bias should be constant and 0. I obtain instead values like 3 or 2 and non constant.
Roll and pitch aren't 0 as they should.
The text I am using to implement the ESKF are Quaternion kinematics for the error-state KF from pag. 52.

Porting Differential Equations from Matlab to Python 3.4 gives different results

I've been trying to port a set of differential equations from Matlab R2014b to python 3.4.
I've used both odeint and ode, with no satisfactory results. The expected results are the ones I get from Matlab where xi = xl and xj = xk with an offset of 180 in phase for each group, as you can see from the image below.
The code I'm running in Matlab is the following:
function Fv = cpg(t, ini_i);
freq = 2;
%fixed parameters
alpha = 5;
beta = 50;
miu = 1;
b = 1;
%initial conditions
xi = ini_i(1);
yi = ini_i(2);
xj = ini_i(3);
yj = ini_i(4);
xk = ini_i(5);
yk = ini_i(6);
xl = ini_i(7);
yl = ini_i(8);
ri = sqrt(xi^2 + yi^2);
rj = sqrt(xj^2 + yj^2);
rk = sqrt(xk^2 + yk^2);
rl = sqrt(xl^2 + yl^2);
%frequency for all oscillators
w_swing = 1;
w_stance = freq*w_swing;
%Coupling matrix, that determines a walking gate for
%a quadruped robot.
k = [ 0, -1, -1, 1;
-1, 0, 1, -1;
-1, 1, 0, -1;
1, -1, -1, 0];
%Hopf oscillator 1
omegai = w_stance/(exp(-b*yi)+1) + w_swing/(exp(b*yi)+1);
xi_dot = alpha*(miu - ri^2)*xi - omegai*yi;
yi_dot = beta*(miu - ri^2)*yi + omegai*xi + k(1,2)*yj + k(1,3)*yk + k(1,4)*yl;
%Hopf oscillator 2
omegaj = w_stance/(exp(-b*yj)+1) + w_swing/(exp(b*yj)+1);
xj_dot = alpha*(miu - rj^2)*xj - omegaj*yj;
yj_dot = beta*(miu - rj^2)*yj + omegaj*xj + k(2,1)*yi + k(2,3)*yk + k(2,4)*yl;
%Hopf oscillator 3
omegak = w_stance/(exp(-b*yk)+1) + w_swing/(exp(b*yj)+1);
xk_dot = alpha*(miu - rk^2)*xk - omegak*yk;
yk_dot = beta *(miu - rk^2)*yk + omegak*xk + k(3,4)*yl + k(3,2)*yj + k(3,1)*yi;
%Hopf oscillator 4
omegal = w_stance/(exp(-b*yl)+1) + w_swing/(exp(b*yl)+1);
xl_dot = alpha*(miu - rl^2)*xl - omegal*yl;
yl_dot = beta *(miu - rl^2)*yl + omegal*xl + k(4,3)*yk + k(4,2)*yj + k(4,1)*yi;
%Outputs
Fv(1,1) = xi_dot;
Fv(2,1) = yi_dot;
Fv(3,1) = xj_dot;
Fv(4,1) = yj_dot;
Fv(5,1) = xk_dot;
Fv(6,1) = yk_dot;
Fv(7,1) = xl_dot;
Fv(8,1) = yl_dot;
However, when I moved the code to python I get an output like this.
In python I ran the ODE solver using the same time step as in Matlab and using solver 'dopri5', which is supposed to be equivalent to the one I'm using in Matlab, ode45. I use the same initial conditions in both cases. I've used both odeint and ode with similar results.
I just started programming in Python and it is my first implementation using Scipy and Numpy so maybe I'm misinterpreting something?
def cpg(t, ini, k, freq):
alpha = 5
beta = 50
miu = 1
b = 1
assert freq == 2
'Initial conditions'
xi = ini[0]
yi = ini[1]
xj = ini[2]
yj = ini[3]
xk = ini[4]
yk = ini[5]
xl = ini[6]
yl = ini[7]
ri = sqrt(xi**2 + yi**2)
rj = sqrt(xj**2 + yj**2)
rk = sqrt(xk**2 + yk**2)
rl = sqrt(xl**2 + yl**2)
'Frequencies for each oscillator'
w_swing = 1
w_stance = freq * w_swing
'First Oscillator'
omegai = w_stance/(exp(-b*yi)+1) + w_swing/(exp(b*yi)+1)
xi_dot = alpha*(miu - ri**2)*xi - omegai*yi
yi_dot = beta*(miu - ri**2)*yi + omegai*xi + k[1]*yj + k[2]*yk + k[3]*yl
'Second Oscillator'
omegaj = w_stance/(exp(-b*yj)+1) + w_swing/(exp(b*yj)+1)
xj_dot = alpha*(miu - rj**2)*xj - omegaj*yj
yj_dot = beta*(miu - rj**2)*yj + omegaj*xj + k[4]*yi + k[6]*yk + k[7]*yl
'Third Oscillator'
omegak = w_stance/(exp(-b*yk)+1) + w_swing/(exp(b*yk)+1)
xk_dot = alpha*(miu - rk**2)*xk - omegak*yk
yk_dot = beta*(miu - rk**2)*yk + omegak*xk + k[8]*yi + k[9]*yj + k[11]*yl
'Fourth Oscillator'
omegal = w_stance/(exp(-b*yl)+1) + w_swing/(exp(b*yl)+1)
xl_dot = alpha*(miu - rl**2)*xl - omegal*yl
yl_dot = beta*(miu - rl**2)*yl + omegal*xl + k[12]*yi + k[13]*yj + k[14]*yk
return [xi_dot, yi_dot, xj_dot, yj_dot, xk_dot, yk_dot, xl_dot, yl_dot]
The way that I'm calling the ODE is as follows:
X0 = [1,-1, 0,-1, 1,1, 0,1]
r = ode(cpg).set_integrator('dopri5')
r.set_initial_value(X0).set_f_params(k_trot, 2)
t1 = 30.
dt = .012
while r.successful() and r.t < (t1-dt):
r.integrate(r.t+dt)
I hope I was clear enough.
Any suggestions?

Integral Calculation

I would like to perform what follows:
where
The parameters shown in the latter figure can be obtained as follows:
%% Inizialization
time = 614.4; % Analysis Time
Uhub = 11;
HubHt = 90;
alpha = 0.14;
TI = 'A'; % Turbulent Intensity (A,B,C as in the IEC or Specific value)
N1 = 4096;
N2 = 32;
N3 = 32;
N = N1*N2*N3; % Total Number of Point
t = 0:(time/(N1-1)):time; % Sampled Time Vector
L1 = Uhub*time; % Box length along X
L2 = 150; % Box length along Y
L3 = 220; % Box length along Z
dx = L1/N1; % Grid Resolution along X-axis
dy = L2/N2; % Grid Resolution along Y-axis
dz = L3/N3; % Grid Resolution along Z-axis
V = L1*L2*L3; % Analysis Box Volume
gamma = 3.9; % Turbulent Eddies Distorsion Factor
c = 1.476;
b = 5.6;
if HubHt < 60
lambda1 = 0.7*HubHt;
else
lambda1 = 42;
end
L = 0.8*lambda1;
if isequal(TI,'A')
Iref = 0.16;
sigma1 = Iref*(0.75*Uhub + b);
elseif isequal(TI,'B')
Iref = 0.14;
sigma1 = Iref*(0.75*Uhub + b);
elseif isequal(TI,'C')
Iref = 0.12;
sigma1 = Iref*(0.75*Uhub + b);
else
sigma1 = str2num(TI)*Uhub/100;
end
sigma_iso = 0.55*sigma1;
sigma2 = 0.7*sigma1;
sigma3 = 0.5*sigma1;
%% Wave number vectors
ik1 = cat(2,(-N1/2:-1/2),(1/2:N1/2));
ik2 = -N2/2:N2/2-1;
ik3 = -N3/2:N3/2-1;
[x y z] = ndgrid(ik1,ik2,ik3);
k1 = reshape((2*pi*L/L1)*x,N1*N2*N3,1);
k2 = reshape((2*pi*L/L2)*y,N1*N2*N3,1);
k3 = reshape((2*pi*L/L3)*z,N1*N2*N3,1);
k = sqrt(k1.^2 + k2.^2 + k3.^2);
%% Calculation of beta by means of the Energy Spectrum Integration
E = #(k) (1.453*k.^4)./((1 + k.^2).^(17/6));
%//Independent integration on segments
Local_int = arrayfun(#(i)quad(E,i-1,i), 2:(N1*N2*N3));
%//integral additivity + cumulative removal of queues
E_int = 1.5 - [0 fliplr(cumsum(fliplr(Local_int)))]; %//To remove queues
E_int = reshape(E_int,N,1);
S = k.*sqrt(E_int);
beta = (c*gamma)./S;
%% Help Parameters
k30 = k3 + beta.*k1;
k0 = sqrt(k1.^2 + k2.^2 + k30.^2);
C1 = (beta.*k1.^2.*(k1.^2 + k2.^2 - k3.*k30))./(k.^2.*(k1.^2 + k2.^2));
C2 = (k2.*k0.^2./((k1.^2 + k2.^2).^(3/2))).*atan2((beta.*k1.*sqrt(k1.^2 + k2.^2)),(k0.^2 - k30.*k1.*beta));
xhsi1 = C1 - (k2./k1).*C2;
xhsi2 = (k2./k1).*C1 + C2;
E_k0 = (1.453*k0.^4)./((1 + k0.^2).^(17/6));
For instance, typing in
phi_33 = #(k2,k3) (E_k0./(4*pi.*k.^4)).*((k1.^2 + k2.^2));
F_33 = arrayfun(#(i) dblquad(phi_33,k3(i),k3(i+1),k2(i),k2(i+1)), 1:((N1*N2*N3)-1));
Matlab retrieves the following error msg:
Error using +
Matrix dimensions must agree.
Error in #(k2,k3)(E_k0./(4*pi.*k.^4)).*((k1.^2+k2.^2))
Do you have a clue how to overcome this issue?
I really look forward to hearing from you.
Best regards,
FPE
The error is easily explained:
First you define E_k0 then you try to call Ek0.
phi_11 = #(k1,k2,k3) (E_k0./4*pi.*kabs.^4).*(k0abs.^2 - k1.^2 - 2*k1.*k03.*xhsi1 + (k1.^2 + k2.^2).*xhsi1.^2);
I solved it this way:
Code a function for each of the PHI elements, such as (for PHI11)
function phi_11 = phi_11_new(k1,k2,k3,beta,i)
k = sqrt(k1(i).^2 + k2.^2 + k3.^2);
k30 = k3 + beta(i).*k1(i);
k0 = sqrt(k1(i).^2 + k2.^2 + k30.^2);
E_k0 = 1.453.*k0.^4./((1 + k0.^2).^(17/6));
C1 = (beta(i).k1(i).^2).(k1(i).^2 + k2.^2 - k3.k30)./(k.^2.(k1(i).^2 + k2.^2));
C2 = k2.*k0.^2./((k1(i).^2 + k2.^2).^(3/2)).*atan2((beta(i).*k1(i).*sqrt(k1(i).^2 + k2.^2)),(k0.^2 - k30.*k1(i).*beta(i)));
xhsi1 = C1 - k2./k1(i).*C2;
xhsi1_q = xhsi1.^2;
phi_11 = E_k0./(4.*pi.k0.^4).(k0.^2 - k1(i).^2 - 2.*k1(i).*k30.*xhsi1 + (k1(i).^2 + k2.^2).*xhsi1_q);
end
I recall this function within the main code as follows
for l = 1:numel(k1)
phi11 = #(k2,k3) phi_11(k1,k2,k3,l)
F11(l) = integral2(phi,-1000,1000,-1000,1000);
end
at it seems it works.