Related
I have a Matlab code that simulates frisbee flight dynamics. I would like to add a wind variable. I did it, but after seeing the plots I think my wind is reducing the speed of the disc. I mean it should change the speed of the disc but via lift and drag force, now it looks like wind speed variable direcly changes disc speed variable. What I want is to affect only the lift and drag forces with wind, but I can't make it work. Here is my current code that is not working. This is an external M-file which is used by the ode45 function in the main script:
[t,x]=ode45(#discfltEOM,tspan,x0,options,CoefUsed);
function xdot=discfltEOM(t,x,CoefUsed)
% Equations of Motion for the frisbee
% The inertial frame, xyz = forward, right and down
global m g Ia Id A d rho
global CLo CLa CDo CDa CMo CMa CRr
global CL_data CD_data CM_data CRr_rad CRr_AdvR CRr_data
global CMq CRp CNr
% x = [ x y z vx vy vz f th fd thd gd gamma Wx Wy]
% 1 2 3 4 5 6 7 8 9 10 11 12 13 14
%% give states normal names
vx = x(4);
vy = x(5);
vz = x(6);
f = x(7);
th = x(8);
st = sin(th);
ct = cos(th);
sf = sin(f);
cf = cos(f);
fd = x(9);
thd= x(10);
gd = x(11);
Wx = x(13);
Wy = x(14);
%% Define transformation matrix
%% [c]=[T_c_N] * [N]
T_c_N=[ct st*sf -st*cf; 0 cf sf; st -ct*sf ct*cf];
%% [d]=[T_d_N] * [N]
%T_d_N(1,:)=[cg*ct sg*cf+sf*st*cg sf*sg-st*cf*cg];
%T_d_N(2,:)=[ -sg*ct cf*cg-sf*sg*st sf*cg+sg*st*cf];
%T_d_N(3,:)=[ st -sf*ct cf*ct]
[evec,eval]=eig(T_c_N);
eigM1=diag(eval);
m1=norm(eigM1(1));
m2=norm(eigM1(2));
m3=norm(eigM1(3));
c1=T_c_N(1,:); % c1 expressed in N frame
c2=T_c_N(2,:); % c2 expressed in N frame
c3=T_c_N(3,:); % c3 expressed in N frame
%% calculate aerodynamic forces and moments
%% every vector is expressed in the N frame
vel = [vx vy vz]; %expressed in N
vmag = norm(vel);
Vwiatr = [Wx Wy 0];
Vw = norm(Vwiatr);
vc3=dot(vel,c3); % velocity (scalar) in the c3 direction
vp= [vel-vc3*c3]; % subtract the c3 velocity component to get the velocity vector
% projected onto the plane of the disc, expressed in N
alpha = atan(vc3/norm(vp));
Adp = A*rho*(vmag-Vw)*(vmag-Vw)/2;
uvel = vel/vmag; % unit vector in vel direction, expressed in N
uvp = vp/norm(vp); % unit vector in the projected velocity direction, expressed in N
ulat = cross(c3,uvp); % unit vec perp to v and d3 that points to right, right?
%% first calc moments in uvp (roll), ulat(pitch) directions, then express in n1,n2,n3
omegaD_N_inC = [fd*ct thd fd*st+gd]; % expressed in c1,c2,c3
omegaD_N_inN = T_c_N'*omegaD_N_inC'; % expressed in n1,n2,n3
omegavp = dot(omegaD_N_inN,uvp);
omegalat = dot(omegaD_N_inN,ulat);
omegaspin = dot(omegaD_N_inN,c3); % omegaspin = p1=fd*st+gd
AdvR= d*omegaspin/2/vmag ; % advanced ration
if CoefUsed==1 % using short flights coefficients
CL = CLo + CLa*alpha;
alphaeq = -CLo/CLa; % this is angle of attack at zero lift
CD = CDo + CDa*(alpha-alphaeq)*(alpha-alphaeq);
CM=CMo + CMa*alpha;
%CRr= CRr*d*omegaspinv/2./vmagv';
%CRr= CRr*sqrt(d/g)*omegaspinv; % this line produces NaN, so leave it in Mvp equation
%Mvp = Adp*d* (CRr*d*omegaspin/2/vmag + CRp*omegavp)*uvp; % expressed in N
Mvp = Adp*d*(sqrt(d/g)*CRr*omegaspin + CRp*omegavp)*uvp; % expressed in N
end % if CoefUsed==1 % using short flights coefficients
if CoefUsed==2 % using potts coefficients
%% interpolation of Potts and Crowther (2002) data
CL = interp1(CL_data(:,1), CL_data(:,2), alpha,'spline');
CD = interp1(CD_data(:,1), CD_data(:,2), alpha,'spline');
CM = interp1(CM_data(:,1), CM_data(:,2), alpha,'spline');
CRr = interp2(CRr_rad,CRr_AdvR,CRr_data,alpha,AdvR,'spline');
Mvp = Adp*d* (CRr* + CRp*omegavp)*uvp; % Roll moment, expressed in N
end % if CoefUsed==2 % using potts coefficients
lift = CL*Adp;
drag = CD*Adp;
ulift = -cross(uvel,ulat); % ulift always has - d3 component
udrag = -uvel;
Faero = lift*ulift + drag*udrag; % aero force in N
FgN = [ 0 0 m*g]'; % gravity force in N
F = Faero' + FgN;
Mlat = Adp*d*(CM + CMq*omegalat)*ulat; % Pitch moment expressed in N
Mspin = [0 0 +CNr*(omegaspin)]; % Spin Down moment expressed in C
M = T_c_N*Mvp' + T_c_N*Mlat' + Mspin'; % Total moment expressed in C
% set moments equal to zero if wanted...
% M=[0 0 0];
% calculate the derivatives of the states
xdot = vel';
xdot(4) = (F(1)/m); %accx
xdot(5) = (F(2)/m); %accy
xdot(6) = (F(3)/m); %accz
xdot(7) = fd;
xdot(8) = thd;
xdot(9) = (M(1) + Id*thd*fd*st - Ia*thd*(fd*st+gd) + Id*thd*fd*st)/Id/ct;
xdot(10) = (M(2) + Ia*fd*ct*(fd*st +gd) - Id*fd*fd*ct*st)/Id;
fdd=xdot(9);
xdot(11) = (M(3) - Ia*(fdd*st + thd*fd*ct))/Ia;
xdot(12) = x(11);
xdot(13) = Wx;
xdot(14) = Wy;
xdott=xdot';
% calculate angular momentum
H = [Id 0 0 ; 0 Id 0; 0 0 Ia]*omegaD_N_inC';
format long;
magH = norm(H);
format short;
state=x';
Wx and Wy are wind vectors. I'm trying to affect the Adp variable because it is direcly connected with lift and drag. I made Wx = 1 [m/s] and the effect is immense, but should be very little. I'm terrible with Matlab so I'm sure I making some kind of stupid mistake from not understanding well how it all works.
When performing the innovation update for Kalman filter, I am getting Warnings
Warning: Matrix is close to singular or badly scaled. Results may be
inaccurate. RCOND = 2.169130e-017.
Maybe due to this, the result is not accurate. How can I solve this problem? I tried introducing a loop
[R,p] = chol(Ppred);
if p> 0;
count = 300;
return;
end
where count is just a variable to stall the code until a good matrix is found. but this does not help.
UPDATE:
Linear system representation of Moving Average, MA(2) model
x_n+1 = Bw_n
y_n = Cx_n + v_n
% w = N(0,Q); v = N(0,R)
%true coefficients, h = [1 0.5 -0.9];
CODE SNIPPETS
These are the functions for the 3 modules of Kalman Filter
C = [ 1 0 0 ];
B = [1 0.5 -0.9 ;
0 1 0.5;
0 0 1];
noise_var = rand(1,1); % measurement noise
order = 2;
xpred = rand(order,1);
P = 10* eye(d,d);
A = P;
P = P + B*sqrt(noise_var)*B';
P = dlyap(A,B*B');
for i = 1:N
[xpred, Ppred] = predict(xpred,B,Ppred, Q);
[nu, S] = innovation(xpred, Ppred, y(i), C, noise_var);
[xnew, Ppred, yhat, KalmanGain] = innovation_update(xpred,Ppred,nu,S,C);
if(isnan(Ppred))
count = 300;
return;
end
end
function [xpred, Ppred] = predict(input_t,B,P, Q)
xpred = B*input_t;
Ppred = P + Q;
end
function [nu, S] = innovation(xpred, Ppred, y, C, R)
nu = y - C*xpred; %% innovation
S = R + C*Ppred*C'; %% innovation covariance
end
function [xnew, Pnew, yhat, K] = innovation_update(xpred, Ppred, nu, S, C)
K1 = Ppred*C';
K = K1'*inv(S);
xnew = xpred + K'*nu; %% new state
Pnew = Ppred - Ppred*K'*C; %% new covariance
yhat = C*xnew;
end
Numerical issues are a common problem with Kalman filters. You didn't share enough of your code to be sure (especially Q), but it is common for roundoff errors to cause P to become non-positive-definite (especially with the P update form you used).
If you google "Kalman filter numerical stability" you can find a lot of references on the subject. Simple things to try in this case would be increasing Q (aka "fictitious process noise") to avoid an ill-conditioned P, using the Joseph form of the covariance update, or forcing P to be symmetric by setting P = 0.5 * ( P + P' ).
More complex options include switching to a square root form (e.g. UDU'), or using a floating point representation with more precision (e.g. double instead of float, which is mainly hard because you're probably already at double).
I've studied "Simulating Ocean Water" article by Jerry Tessendorf and tried to program the Statistical Wave Model but I didn't get correct result and I don't understand why.
In my program I tried only to create a wave height field at time t = 0 without any further changes in time. After execution of my program I got not what I was expecting:
Here's my source code:
clear all; close all; clc;
rng(11); % setting seed for random numbers
meshSize = 64; % field size
windDir = [1, 0]; % ||windDir|| = 1
patchSize = 64;
A = 1e+4;
g = 9.81; % gravitational constant
windSpeed = 1e+2;
x1 = linspace(-10, 10, meshSize+1); x = x1(1:meshSize);
y1 = linspace(-10, 10, meshSize+1); y = y1(1:meshSize);
[X,Y] = meshgrid(x, y);
H0 = zeros(size(X)); % height field at time t = 0
for i = 1:meshSize
for j = 1:meshSize
kx = 2.0 * pi / patchSize * (-meshSize / 2.0 + x(i)); % = 2*pi*n / Lx
ky = 2.0 * pi / patchSize * (-meshSize / 2.0 + y(j)); % = 2*pi*m / Ly
P = phillips(kx, ky, windDir, windSpeed, A, g); % phillips spectrum
H0(i,j) = 1/sqrt(2) * (randn(1) + 1i * randn(1)) * sqrt(P);
end
end
H0 = H0 + conj(H0);
surf(X,Y,abs(ifft(H0)));
axis([-10 10 -10 10 -10 10]);
And the phillips function:
function P = phillips(kx, ky, windDir, windSpeed, A, g)
k_sq = kx^2 + ky^2;
L = windSpeed^2 / g;
k = [kx, ky] / sqrt(k_sq);
wk = k(1) * windDir(1) + k(2) * windDir(2);
P = A / k_sq^2 * exp(-1.0 / (k_sq * L^2)) * wk^2;
end
Is there any matlab ocean simulation source code which could help me to understand my mistakes? Fast google search didn't get any results.
Here's a "correct" result I got from "CUDA FFT Ocean Simulation". I didn't achieve this behavior in Matlab yet but I've ploted "surf" in matlab using data from "CUDA FFT Ocean Simulation". Here's what it looks like:
I've made an experiment and got an interesting result:
I've taken generated h0 from "CUDA FFT Ocean Simulation". So I have to do ifft to transform from frequency domain to spatial domain to plot the graph. I've done it for the same h0 using matlab ifft and using cufftExecC2C from CUDA library. Here's the result:
CUDA ifft:
Matlab ifft:
Either I don't understand some aspects of realization of cufftExecC2C or cufftExecC2C and matlab ifft are different algorithms with different results.
By the way parameters for generating such surface are:
meshSize = 32
A = 1e-7
patchSize = 80
windSpeed = 10
Well that was definitely a funny exercise. This is a completely rewritten answer since you found the issues you were asking about by yourself.
Instead of deleting my answer, there is still merit in posting to help you vectorize and/or explain a few bits of code.
I completely rewrote the GUI I gave in my former answer in order to incorporate your changes and add a couple of options. It started to grew arms and legs so I won't put the listing here but you can find the full file there:
ocean_simulator.m.
This is completely self contained and it includes all the calculating functions I vectorized and list separately below.
The GUI will allow you to play with the parameters, animate the waves, export GIF file (and a few other options like the "preset", but they are not too ironed out yet). A few examples of what you can achieve:
Basic
This is what you get with the quick default settings, and a couple of rendering options. This uses a small grid size and a fast time step, so it runs pretty quickly on any machine.
I am quite limited at home (Pentium E2200 32bit), so I could only practice with limited settings. The gui will run even with the settings maxed but it will become to slow to really enjoy.
However, with a quick run of ocean_simulator at work (I7 64 bit, 8 cores, 16GB ram, 2xSSD in Raid), it makes it much more fun! Here are a few examples:
Although done on a much better machine, I didn't use any parallel functionality nor any GPU calculations, so Matlab was only using a portion of these specs, which means it could probably run just as good on any 64bit system with decent RAM
Windy lake
This is a rather flat water surface like a lake. Even high winds do not produce high amplitude waves (but still a lot of mini wavelets). If you're a wind surfer looking at that from your window on top of the hill, your heart is going to skip a beat and your next move is to call Dave "Man! gear up. Meet you in five on the water!"
Swell
This is you looking from the bridge of your boat on the morning, after having battled with the storm all night. The storm has dissipated and the long large waves are the last witness of what was definitely a shaky night (people with sailing experience will know ...).
T-Storm
And this was what you were up to the night before...
second gif done at home, hence the lack of detail ... sorry
To the bottom:
Finally, the gui will let you add a patch around the water domain. In the gui it is transparent so you could add objects underwater or a nice ocean bottom. Unfortunately, the GIF format cannot include an alpha channel so no transparency here (but if you export in a video then you should be ok).
Moreover, the export to GIF degrade the image, the joint between the domain border and the water surface is flawless if you run that in Matlab. In some case it also make Matlab degrade the rendering of the lighting, so this is definitely not the best option for export, but it allows more things to play within matlab.
Now onto the code:
Instead of listing the full GUI, which would be super long (this post is long enough already), I will just list here the re-written version of your code, and explain the changes.
You should notice a massive increase of speed execution (orders of magnitude), thanks to the remaining vectorization, but mostly for two reasons:
(i) A lot of calculations were repeated. Caching values and reusing them is much faster than recalculating full matrices in loops (during the animation part).
(ii) Note how I defined the surface graphic object. It is defined only once (empty even), then all the further calls (in the loop) only update the underlying ZData of the surface object (instead of re-creating a surface object at each iteration.
Here goes:
%% // clear workspace
clear all; close all; clc;
%% // Default parameters
param.meshsize = 128 ; %// main grid size
param.patchsize = 200 ;
param.windSpeed = 100 ; %// what unit ? [m/s] ??
param.winddir = 90 ; %// Azimuth
param.rng = 13 ; %// setting seed for random numbers
param.A = 1e-7 ; %// Scaling factor
param.g = 9.81 ; %// gravitational constant
param.xLim = [-10 10] ; %// domain limits X
param.yLim = [-10 10] ; %// domain limits Y
param.zLim = [-1e-4 1e-4]*2 ;
gridSize = param.meshsize * [1 1] ;
%% // Define the grid X-Y domain
x = linspace( param.xLim(1) , param.xLim(2) , param.meshsize ) ;
y = linspace( param.yLim(1) , param.yLim(2) , param.meshsize ) ;
[X,Y] = meshgrid(x, y);
%% // get the grid parameters which remain constants (not time dependent)
[H0, W, Grid_Sign] = initialize_wave( param ) ;
%% // calculate wave at t0
t0 = 0 ;
Z = calc_wave( H0 , W , t0 , Grid_Sign ) ;
%% // populate the display panel
h.fig = figure('Color','w') ;
h.ax = handle(axes) ; %// create an empty axes that fills the figure
h.surf = handle( surf( NaN(2) ) ) ; %// create an empty "surface" object
%% // Display the initial wave surface
set( h.surf , 'XData',X , 'YData',Y , 'ZData',Z )
set( h.ax , 'XLim',param.xLim , 'YLim',param.yLim , 'ZLim',param.zLim )
%% // Change some rendering options
axis off %// make the axis grid and border invisible
shading interp %// improve shading (remove "faceted" effect)
blue = linspace(0.4, 1.0, 25).' ; cmap = [blue*0, blue*0, blue]; %'// create blue colormap
colormap(cmap)
%// configure lighting
h.light_handle = lightangle(-45,30) ; %// add a light source
set(h.surf,'FaceLighting','phong','AmbientStrength',.3,'DiffuseStrength',.8,'SpecularStrength',.9,'SpecularExponent',25,'BackFaceLighting','unlit')
%% // Animate
view(75,55) %// no need to reset the view inside the loop ;)
timeStep = 1./25 ;
nSteps = 2000 ;
for time = (1:nSteps)*timeStep
%// update wave surface
Z = calc_wave( H0,W,time,Grid_Sign ) ;
h.surf.ZData = Z ;
pause(0.001);
end
%% // This block of code is only if you want to generate a GIF file
%// be carefull on how many frames you put there, the size of the GIF can
%// quickly grow out of proportion ;)
nFrame = 55 ;
gifFileName = 'MyDancingWaves.gif' ;
view(-70,40)
clear im
f = getframe;
[im,map] = rgb2ind(f.cdata,256,'nodither');
im(1,1,1,20) = 0;
iframe = 0 ;
for time = (1:nFrame)*.5
%// update wave surface
Z = calc_wave( H0,W,time,Grid_Sign ) ;
h.surf.ZData = Z ;
pause(0.001);
f = getframe;
iframe= iframe+1 ;
im(:,:,1,iframe) = rgb2ind(f.cdata,map,'nodither');
end
imwrite(im,map,gifFileName,'DelayTime',0,'LoopCount',inf)
disp([num2str(nFrame) ' frames written in file: ' gifFileName])
You'll notice that I changed a few things, but I can assure you the calculations are exactly the same. This code calls a few subfunctions but they are all vectorized so if you want you can just copy/paste them here and run everything inline.
The first function called is initialize_wave.m
Everything calculated here will be constant later (it does not vary with time when you later animate the waves), so it made sense to put that into a block on it's own.
function [H0, W, Grid_Sign] = initialize_wave( param )
% function [H0, W, Grid_Sign] = initialize_wave( param )
%
% This function return the wave height coefficients H0 and W for the
% parameters given in input. These coefficients are constants for a given
% set of input parameters.
% Third output parameter is optional (easy to recalculate anyway)
rng(param.rng); %// setting seed for random numbers
gridSize = param.meshsize * [1 1] ;
meshLim = pi * param.meshsize / param.patchsize ;
N = linspace(-meshLim , meshLim , param.meshsize ) ;
M = linspace(-meshLim , meshLim , param.meshsize ) ;
[Kx,Ky] = meshgrid(N,M) ;
K = sqrt(Kx.^2 + Ky.^2); %// ||K||
W = sqrt(K .* param.g); %// deep water frequencies (empirical parameter)
[windx , windy] = pol2cart( deg2rad(param.winddir) , 1) ;
P = phillips(Kx, Ky, [windx , windy], param.windSpeed, param.A, param.g) ;
H0 = 1/sqrt(2) .* (randn(gridSize) + 1i .* randn(gridSize)) .* sqrt(P); % height field at time t = 0
if nargout == 3
Grid_Sign = signGrid( param.meshsize ) ;
end
Note that the initial winDir parameter is now expressed with a single scalar value representing the "azimuth" (in degrees) of the wind (anything from 0 to 360). It is later translated to its X and Y components thanks to the function pol2cart.
[windx , windy] = pol2cart( deg2rad(param.winddir) , 1) ;
This insure that the norm is always 1.
The function calls your problematic phillips.m separately, but as said before it works even fully vectorized so you can copy it back inline if you like. (don't worry I checked the results against your versions => strictly identical). Note that this function does not output complex numbers so there was no need to compare the imaginary parts.
function P = phillips(Kx, Ky, windDir, windSpeed, A, g)
%// The function now accept scalar, vector or full 2D grid matrix as input
K_sq = Kx.^2 + Ky.^2;
L = windSpeed.^2 ./ g;
k_norm = sqrt(K_sq) ;
WK = Kx./k_norm * windDir(1) + Ky./k_norm * windDir(2);
P = A ./ K_sq.^2 .* exp(-1.0 ./ (K_sq * L^2)) .* WK.^2 ;
P( K_sq==0 | WK<0 ) = 0 ;
end
The next function called by the main program is calc_wave.m. This function finishes the calculations of the wave field for a given time. It is definitely worth having that on its own because this is the mimimun set of calculations which will have to be repeated for each given time when you want to animate the waves.
function Z = calc_wave( H0,W,time,Grid_Sign )
% Z = calc_wave( H0,W,time,Grid_Sign )
%
% This function calculate the wave height based on the wave coefficients H0
% and W, for a given "time". Default time=0 if not supplied.
% Fourth output parameter is optional (easy to recalculate anyway)
% recalculate the grid sign if not supplied in input
if nargin < 4
Grid_Sign = signGrid( param.meshsize ) ;
end
% Assign time=0 if not specified in input
if nargin < 3 ; time = 0 ; end
wt = exp(1i .* W .* time ) ;
Ht = H0 .* wt + conj(rot90(H0,2)) .* conj(wt) ;
Z = real( ifft2(Ht) .* Grid_Sign ) ;
end
The last 3 lines of calculations require a bit of explanation as they received the biggest changes (all for the same result but a much better speed).
Your original line:
Ht = H0 .* exp(1i .* W .* (t * timeStep)) + conj(flip(flip(H0,1),2)) .* exp(-1i .* W .* (t * timeStep));
recalculate the same thing too many times to be efficient:
(t * timeStep) is calculated twice on the line, at each loop, while it is easy to get the proper time value for each line when time is initialised at the beginning of the loop for time = (1:nSteps)*timeStep.
Also note that exp(-1i .* W .* time) is the same than conj(exp(1i .* W .* time)). Instead of doing 2*m*n multiplications to calculate them each, it is faster to calculate one once, then use the conj() operation which is much faster.
So your single line would become:
wt = exp(1i .* W .* time ) ;
Ht = H0 .* wt + conj(flip(flip(H0,1),2)) .* conj(wt) ;
Last minor touch, flip(flip(H0,1),2)) can be replaced by rot90(H0,2) (also marginally faster).
Note that because the function calc_wave is going to be repeated extensively, it is definitely worth reducing the number of calculations (as we did above), but also by sending it the Grid_Sign parameter (instead of letting the function recalculate it every iteration). This is why:
Your mysterious function signCor(ifft2(Ht),meshSize)), simply reverse the sign of every other element of Ht. There is a faster way of achieving that: simply multiply Ht by a matrix the same size (Grid_Sign) which is a matrix of alternated +1 -1 ... and so on.
so signCor(ifft2(Ht),meshSize) becomes ifft2(Ht) .* Grid_Sign.
Since Grid_Sign is only dependent on the matrix size, it does not change for each time in the loop, you only calculate it once (before the loop) then use it as it is for every other iteration. It is calculated as follow (vectorized, so you can also put it inline in your code):
function sgn = signGrid(n)
% return a matrix the size of n with alternate sign for every indice
% ex: sgn = signGrid(3) ;
% sgn =
% -1 1 -1
% 1 -1 1
% -1 1 -1
[x,y] = meshgrid(1:n,1:n) ;
sgn = ones( n ) ;
sgn(mod(x+y,2)==0) = -1 ;
end
Lastly, you will notice a difference in how the grids [Kx,Ky] are defined between your version and this one. They do produce slightly different result, it's just a matter of choice.
To explain with a simple example, let's consider a small meshsize=5. Your way of doing things will split that into 5 values, equally spaced, like so:
Kx(first line)=[-1.5 -0.5 0.5 1.5 2.5] * 2 * pi / patchSize
while my way of producing the grid will produce equally spaced values, but also centered on the domain limits, like so:
Kx(first line)=[-2.50 -1.25 0.0 1.25 2.50] * 2 * pi / patchSize
It seems to respect more your comment % = 2*pi*n / Lx, -N/2 <= n < N/2 on the line where you define it.
I tend to prefer symmetric solutions (plus it is also slightly faster but it is only calculated once so it is not a big deal), so I used my vectorized way, but it is purely a matter of choice, you can definitely keep your way, it only ever so slightly "offset" the whole result matrix, but it doesn't perturbate the calculations per se.
last remains of the first answer
Side programming notes:
I detect you come from the C/C++ world or family. In Matlab you do not need to define decimal number with a coma (like 2.0, you used that for most of your numbers). Unless specifically defined otherwise, Matlab by default cast any number to double, which is a 64 bit floating point type. So writing 2 * pi is enough to get the maximum precision (Matlab won't cast pi as an integer ;-)), you do not need to write 2.0 * pi. Although it will still work if you don't want to change your habits.
Also, (one of the great benefit of Matlab), adding . before an operator usually mean "element-wise" operation. You can add (.+), substract (.-), multiply (.*), divide (./) full matrix element wise this way. This is how I got rid of all the loops in your code. This also work for the power operator: A.^2 will return a matrix the same size as A with every element squared.
Here's the working program.
First of all - source code:
clear all; close all; clc;
rng(13); % setting seed for random numbers
meshSize = 128; % field size
windDir = [0.1,1];
patchSize = 200;
A = 1e-7;
g = 9.81; % gravitational constant
windSpeed = 100;
timeStep = 1/25;
x1 = linspace(-10, 10, meshSize+1); x = x1(1:meshSize);
y1 = linspace(-10, 10, meshSize+1); y = y1(1:meshSize);
[X,Y] = meshgrid(x,y); % wave field
i = 1:meshSize; j = 1:meshSize; % indecies
[I,J] = meshgrid(i,j); % field of indecies
Kx = 2.0 * pi / patchSize * (-meshSize / 2.0 + I); % = 2*pi*n / Lx, -N/2 <= n < N/2
Ky = 2.0 * pi / patchSize * (-meshSize / 2.0 + J); % = 2*pi*m / Ly, -M/2 <= m < M/2
K = sqrt(Kx.^2 + Ky.^2); % ||K||
W = sqrt(K .* g); % deep water frequencies (empirical parameter)
P = zeros(size(X)); % Cant compute P without loops
for i = 1:meshSize
for j = 1:meshSize
P(i,j) = phillips(Kx(i,j), Ky(i,j), windDir, windSpeed, A, g); % phillips spectrum
end
end
H0 = 1/sqrt(2) .* (randn(size(X)) + 1i .* randn(size(X))) .* sqrt(P); % height field at time t = 0
rotate3d on;
for t = 1:10000 % 10000 * timeStep (sec)
Ht = H0 .* exp(1i .* W .* (t * timeStep)) + ...
conj(flip(flip(H0,1),2)) .* exp(-1i .* W .* (t * timeStep));
[az,el] = view;
surf(X,Y,real(signCor(ifft2(Ht),meshSize)));
axis([-10 10 -10 10 -1e-4 1e-4]); view(az,el);
blue = linspace(0.4, 1.0, 25)'; map = [blue*0, blue*0, blue];
%shading interp; % improve shading (remove "faceted" effect)
colormap(map);
pause(1/60);
end
phillips.m: (I've tried to vectorize the computation of Phillips spectrum but I faced with a difficulty which I'll show further)
function P = phillips(kx, ky, windDir, windSpeed, A, g)
k_sq = kx^2 + ky^2;
if k_sq == 0
P = 0;
else
L = windSpeed^2 / g;
k = [kx, ky] / sqrt(k_sq);
wk = k(1) * windDir(1) + k(2) * windDir(2);
P = A / k_sq^2 * exp(-1.0 / (k_sq * L^2)) * wk^2;
if wk < 0
P = 0;
end
end
end
signCor.m: (This function is an absolutely mystery for me... I've copied it from "CUDA FFT Ocean Simulation" realization. Simulation works much worse without it. And again I don't know how to vectorize this function.)
function H = signCor(H1, meshSize)
H = H1;
for i = 1:meshSize
for j = 1:meshSize
if mod(i+j,2) == 0
sign = -1; % works fine if we change signs vice versa
else
sign = 1;
end
H(i,j) = H1(i,j) * sign;
end
end
end
The biggest mistake that I've done is that I used ifft instead of using ifft2, that's why CUDA ifft and Matlab ifft didn't match.
My second mistake was in this lines of code:
kx = 2.0 * pi / patchSize * (-meshSize / 2.0 + x(i)); % = 2*pi*n / Lx
ky = 2.0 * pi / patchSize * (-meshSize / 2.0 + y(j)); % = 2*pi*m / Ly
I should've write:
kx = 2.0 * pi / patchSize * (-meshSize / 2.0 + i); % = 2*pi*n / Lx
ky = 2.0 * pi / patchSize * (-meshSize / 2.0 + j); % = 2*pi*m / Ly
I've played a bit with parameters A, meshSize, patchSize and I came to the conclusion that:
Somehow plausible parameter of wave amplitude is A * (patchSize / meshSize), where A is nothing but a scaling factor.
For 'calm' patchSize / meshSize <= 0.5.
For 'tsunami' patchSize / meshSize >= 3.0.
Difficulty with a vectorization of Phillips spectrum:
I have 2 functions:
% non-vectorized spectrum
function P = phillips1(kx, ky, windDir, windSpeed, A, g)
k_sq = kx^2 + ky^2;
if k_sq == 0
P = 0;
else
L = windSpeed^2 / g;
k = [kx, ky] / sqrt(k_sq);
wk = k(1) * windDir(1) + k(2) * windDir(2);
P = A / k_sq^2 * exp(-1.0 / (k_sq * L^2)) * wk^2;
if wk < 0
P = 0;
end
end
end
% vectorized spectrum
function P = phillips2(Kx, Ky, windDir, windSpeed, A, g)
K_sq = Kx .^ 2 + Ky .^ 2;
L = -g^2 / windSpeed^4;
WK = (Kx ./ K_sq) .* windDir(1) + (Ky ./ K_sq) .* windDir(2);
P = (A ./ (K_sq .^ 2)) .* ( exp(L ./ K_sq) .* (WK .^ 2) );
P(K_sq == 0) = 0;
P(WK < 0) = 0;
P(isinf(P)) = 0;
end
After I compute P1 using phillips1 and P2 using phillips2 I plot their difference:
subplot(2,1,1); surf(X,Y,real(P2-P1)); title('Difference in real part');
subplot(2,1,2); surf(X,Y,imag(P2-P1)); title('Difference in imaginary part');
It perfectly illustrates that there's a huge difference between this 2 spectrums in real part.
So, I'm implementing the EM algorithm in Matlab, but my matrices quickly end up contaminated by NaN and Inf values. I think it might be caused by matrix inversions, but I'm not sure that's the only reason.
Here is the code:
function [F, Q, R, x_T, P_T] = em_algo(y, G)
% y_t = G_t'*x_t + v_t 1*1 = 1*p p*1
% x_t = F*x_t-1 + w_t p*1 = p*p p*1
% G is T*p
p = size(G,2); % p = nb assets ; G = T*p
q = size(y,2); % q = nb observations ; y = T*q
T = size(y,1); % y is T*1
F = eye(p); % = Transition matrix p*p
Q = eye(p); % innovation (v) covariance matrix p*p
R = eye(q); % noise (w) covariance matrix q x q
x_T_old = zeros(p,T);
mu0 = zeros(p,1);
Sigma = eye(p); % Initial state covariance matrix p*p
converged = 0;
i = 0;
max_iter = 60; % only for testing purposes
while ~converged
if i > max_iter
break;
end
% E step = smoothing
fprintf('Iteration %d\n',i);
[x_T,P_T,P_Tm2] = smoother(G,F,Q,R,mu0,Sigma,y);
%x_T
% M step
A = zeros(p,p);
B = zeros(p,p);
C = zeros(p,p);
R = eye(q);
for t = 2:T % eq (9) in EM paper
A = A + (P_T(:,:,t-1) + (x_T(:,t-1)*x_T(:,t-1)'));
end
for t = 2:T % eq (10)
%B = B + (P_Tm2(:,:,t-1) + (x_T(:,t)*x_T(:,t-1)'));
B = B + (P_Tm2(:,:,t) + (x_T(:,t)*x_T(:,t-1)'));
end
for t = 1:T %eq (11)
C = C + (P_T(:,:,t) + (x_T(:,t)*x_T(:,t)'));
end
F = B*inv(A); %eq (12)
Q = (1/T)*(C - (B*inv(A)*B')); % eq (13) pxp
for t = 1:T
bias = y(t) - (G(t,:)*x_T(:,t));
R = R + ((bias*bias') + (G(t,:)*P_T(:,:,t)*G(t,:)'));
end
R = (1/T)*R;
if i>1
err = norm(x_T-x_T_old)/norm(x_T_old);
if err < 1e-4
converged = 1;
end
end
x_T_old = x_T;
i = i+1;
end
fprintf('EM algorithm iterated %d times\n',i);
end
This iterates until convergence (which never happens due to my issue) and calls smoother.m at each iteration:
function [x_T, P_T, P_Tm2] = smoother(G,F,Q,R,mu0,Sigma,y)
% G is T*p
p = size(mu0,1); % mu0 is p*1
T = size(y,1); % y is T*1
J = zeros(p,p,T);
K = zeros(p,T); % gain matrix
x = zeros(p,T);
x(:,1) = mu0;
x_m1 = zeros(p,T);
x_T = zeros(p,T); % x values when we know all the data
% Notation : x = xt given t ; x_m1 = xt given t-1 (m1 stands for minus
% one)
P = zeros(p,p,T);% array of cov(xt|y1...yt), eq (6) in Shumway & Stoffer 1982
P(:,:,1) = Sigma;
P_m1 = zeros(p,p,T); % Same notation ; = cov(xt, xt-1|y1...yt) , eq (7)
P_T = zeros(p,p,T);
P_Tm2 = zeros(p,p,T); % cov(xT, xT-1|y1...yT)
for t = 2:T %starts at t = 2 because at each time t we need info about t-1
x_m1(:,t) = F*x(:,t-1); % eq A3 ; pxp * px1 = px1
P_m1(:,:,t) = (F*P(:,:,t-1)*F') + Q; % A4 ; pxp * pxp = pxp
if nnz(isnan(P_m1(:,:,t)))
error('NaNs in P_m1 at time t = %d',t);
end
if nnz(isinf(P_m1(:,:,t)))
error('Infs in P_m1 at time t = %d',t);
end
K(:,t) = P_m1(:,:,t)*G(t,:)'*pinv((G(t,:)*P_m1(:,:,t)*G(t,:)') + R); %A5 ; pxp * px1 * 1*1 = p*1
%K(:,t) = P_m1(:,:,t)*G(t,:)'/((G(t,:)*P_m1(:,:,t)*G(t,:)') + R); %A5 ; pxp * px1 * 1*1 = p*1
% The matrix inversion seems to generate NaN values which quickly
% contaminate all the other matrices. There is no warning about
% (close to) singular matrices or whatever. The use of pinv()
% instead of inv() seems to solve the problem... but I don't think
% it's the appropriate way to deal with it, there must be something
% wrong elsewhere
if nnz(isnan(K(:,t)))
error('NaNs in K at time t = %d',t);
end
x(:,t) = x_m1(:,t) + (K(:,t)*(y(t)-(G(t,:)*x_m1(:,t)))); %A6
P(:,:,t) = P_m1(:,:,t) - (K(:,t)*G(t,:)*P_m1(:,:,t)); %A7
end
x_T(:,T) = x(:,T);
P_T(:,:,T) = P(:,:,T);
for t = T:-1:2 % we stop at 2 since we need to use t-1
%P_m1 seem to get really huge (x10^22...), might lead to "Inf"
%values which in turn might screw pinv()
%% inv() caused NaN value to appear, pinv seems to solve the issue
J(:,:,t-1) = P(:,:,t-1)*F'*pinv(P_m1(:,:,t)); % A8 pxp * pxp * pxp
%J(:,:,t-1) = P(:,:,t-1)*F'/(P_m1(:,:,t)); % A8 pxp * pxp * pxp
x_T(:,t-1) = x(:,t-1) + J(:,:,t-1)*(x_T(:,t)-(F*x(:,t-1))); %A9 % Becomes NaN during 8th iteration!
P_T(:,:,t-1) = P(:,:,t-1) + J(:,:,t-1)*(P_T(:,:,t)-P_m1(:,:,t))*J(:,:,t-1)'; %A10
nans = [nnz(isnan(J)) nnz(isnan(P_m1)) nnz(isnan(F)) nnz(isnan(x_T)) nnz(isnan(x_m1))];
if nnz(nans)
error('NaN invasion at time t = %d',t);
end
end
P_Tm2(:,:,T) = (eye(p) - K(:,T)*G(T,:))*F*P(:,:,T-1); % %A12
for t = T:-1:3 % stop at 3 because use of t-2
P_Tm2(:,:,t-1) = P_m1(:,:,t-1)*J(:,:,t-2)' + J(:,:,t-1)*(P_Tm2(:,:,t)-F*P(:,:,t-1))*J(:,:,t-2)'; % A11
end
end
The NaNs and Infs start popping around the ~8th iteration.
I guess in there somewhere I'm doing something unholy with my matrices, but I really have no clue about what's wrong. I trust your expertise.
Thanks in advance for the help.
Rody :
Here is how I generate the data (it's not "real world" data yet, just some test data generated to check that nothing goes wront) :
T = 500;
nbassets = 3;
G = .1 + randn(T,nbassets); % random walk trajectories
y = (1:T).';
y = 1.01.^y; % 1 * T % Exponential 1% returns curve
Dan :
You're right. I indeed lack the math background to really understand how the formulas are derived. I know it doesn't help, but I'm not sure I can remedy that for the time being. :/
Rody : Yes indeed, I arrived at the same conclusion. But I really have no clue what makes it go wrong like that.
Here is a link to the paper :
http://www.stat.pitt.edu/stoffer/em.pdf
The formulas for the smoother are all at the very end, in the appendix. Thanks for your time so far.
As the user appears to have inserted the answer into his question I will post it here:
As mentioned by #Rody the cause of the problem was that the use of inv created NaN or Inf values.
The user 'solved' this by using pinv instead.
I use convolution and for loops (too much for loops) for calculating the interpolation using
Lagrange's method , here's the main code :
function[p] = lagrange_interpolation(X,Y)
L = zeros(n);
p = zeros(1,n);
% computing L matrice, so that each row i holds the polynom L_i
% Now we compute li(x) for i=0....n ,and we build the polynomial
for k=1:n
multiplier = 1;
outputConv = ones(1,1);
for index = 1:n
if(index ~= k && X(index) ~= X(k))
outputConv = conv(outputConv,[1,-X(index)]);
multiplier = multiplier * ((X(k) - X(index))^-1);
end
end
polynimialSize = length(outputConv);
for index = 1:polynimialSize
L(k,n - index + 1) = outputConv(polynimialSize - index + 1);
end
L(k,:) = multiplier .* L(k,:);
end
% continues
end
Those are too much for loops for computing the l_i(x) (this is done before the last calculation of P_n(x) = Sigma of y_i * l_i(x)) .
Any suggestions into making it more matlab formal ?
Thanks
Yeah, several suggestions (implemented in version 1 below): if loop can be combined with for above it (just make index skip k via something like jr(jr~=j) below); polynomialSize is always equal length(outputConv) which is always equal n (because you have n datapoints, (n-1)th polynomial with n coefficients), so the last for loop and next line can be also replaced with simple L(k,:) = multiplier * outputConv;
So I replicated the example on http://en.wikipedia.org/wiki/Lagrange_polynomial (and adopted their j-m notation, but for me j goes 1:n and m is 1:n and m~=j), hence my initialization looks like
clear; clc;
X=[-9 -4 -1 7]; %example taken from http://en.wikipedia.org/wiki/Lagrange_polynomial
Y=[ 5 2 -2 9];
n=length(X); %Lagrange basis polinomials are (n-1)th order, have n coefficients
lj = zeros(1,n); %storage for numerator of Lagrange basis polyns - each w/ n coeff
Lj = zeros(n); %matrix of Lagrange basis polyns coeffs (lj(x))
L = zeros(1,n); %the Lagrange polynomial coefficients (L(x))
then v 1.0 looks like
jr=1:n; %j-range: 1<=j<=n
for j=jr %my j is your k
multiplier = 1;
outputConv = 1; %numerator of lj(x)
mr=jr(jr~=j); %m-range: 1<=m<=n, m~=j
for m = mr %my m is your index
outputConv = conv(outputConv,[1 -X(m)]);
multiplier = multiplier * ((X(j) - X(m))^-1);
end
Lj(j,:) = multiplier * outputConv; %jth Lagrange basis polinomial lj(x)
end
L = Y*Lj; %coefficients of Lagrange polinomial L(x)
which can be further simplified if you realize that numerator of l_j(x) is just a polynomial with specific roots - for that there is a nice command in matlab - poly. Similarly the denominator is just that polyn evaluated at X(j) - for that there is polyval. Hence, v 1.9:
jr=1:n; %j-range: 1<=j<=n
for j=jr
mr=jr(jr~=j); %m-range: 1<=m<=n, m~=j
lj=poly(X(mr)); %numerator of lj(x)
mult=1/polyval(lj,X(j)); %denominator of lj(x)
Lj(j,:) = mult * lj; %jth Lagrange basis polinomial lj(x)
end
L = Y*Lj; %coefficients of Lagrange polinomial L(x)
Why version 1.9 and not 2.0? well, there is probably a way to get rid of this last for loop, and write it all in 1 line, but I can't think of it right now - it's a todo for v 2.0 :)
And, for dessert, if you want to get the same picture as wikipedia:
figure(1);clf
x=-10:.1:10;
hold on
plot(x,polyval(Y(1)*Lj(1,:),x),'r','linewidth',2)
plot(x,polyval(Y(2)*Lj(2,:),x),'b','linewidth',2)
plot(x,polyval(Y(3)*Lj(3,:),x),'g','linewidth',2)
plot(x,polyval(Y(4)*Lj(4,:),x),'y','linewidth',2)
plot(x,polyval(L,x),'k','linewidth',2)
plot(X,Y,'ro','linewidth',2,'markersize',10)
hold off
xlim([-10 10])
ylim([-10 10])
set(gca,'XTick',-10:10)
set(gca,'YTick',-10:10)
grid on
produces
enjoy and feel free to reuse/improve
Try:
X=0:1/20:1; Y=cos(X) and create L and apply polyval(L,1).
polyval(L,1)=0.917483227909543
cos(1)=0.540302305868140
Why there is huge difference?