Related
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 trying to simulate a motion of some particles.The program seems to be very slow.
I just started lean programming so I dont know where is the problem exactly that make it slow, I think that plotting is take much time.
could some sone suggest me how to improve it?
function folks(N , T)
if nargin < 1
N = 50;
T = 100;
end
A=20;R=50;a=100;r=2;
x0=10*randn(3,N);
v0=0*randn(3,N);
clear c
% Initilazing plot
color = hsv(N);
xh=zeros(1,N);
f=2*max(max(x0));ff=f/1000000;
figure(2),clf
set(gcf,'doublebuffer','on')
hold on, grid on, axis([-1 1 -1 1 -.5 .5]*f)
for j = 1:N
xh(j) = line(x0(1,j),x0(2,j),x0(3,j),'color',color(j,:), ...
'marker','.','markersize',15);
end
title('t = 0','fontsize',18)
rotate3d;
view([1.8,-1.8,1])
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Solving the ode system
tol = 3e-14;
opts = odeset('reltol',tol,'abstol',tol);
X0=[x0(:);v0(:)];
dt=T/1000;
for tnext = dt:dt:10*T
tspan = [tnext-dt tnext];
[T1,X1] = ode45(#odefolk,tspan,X0,opts,r,R,a,A);
X0 = X1(end,:);
if max(abs(X0(1:3*N)))>f
f=1.1*f;
axis([-1 1 -1 1 -1 1]*f)
end
for j=0:N-1
set(xh(j+1),'xdata',X0(3*j+1),'ydata',X0(3*j+2),'zdata',X0(3*j+3))
end
title(sprintf('t = %3.0f %5.0e',tnext),'fontsize',18),
drawnow
end
I tried to save the data in a matrix and then plot every thing in new loop but it make it much slower.
And also its not related to the solver ode45 or the system odefolk
function dx = fkt(~,x,r,R,a,A)
n = length(x); % n = 90 = 6 *N
M = n/6; % M= 15
dx = zeros(n,1);
for i = 1 : 3 : n/2
vx = 0 ; vy = 0 ; vz = 0;
for j = 1 : 3 : n/2
if (i~=j)
vx = x(i) - x(j) ;
vy = x(i+1) - x(j+1);
vz = x(i+2) - x(j+2);
% ex = expo (vx , vy , vz ,r , a);
%
% val = (ex(1) * R - ex(2) * A);
leng = sqrt(vx^ 2 +vy^ 2 + vz^ 2);
expo1 = exp(-1 * leng / r) / r;
expo2 = exp(-1 * leng / a) / a;
val = (expo1 * R - expo2 * A);
vx = vx * val;
vy = vy * val;
vz = vz * val;
end
end
vx = vx/M ;
vy = vy/M;
vz = vz/M;
dx(i) = x(i + 3 * M );
dx(i + 1) = x(i + 3 * M + 1);
dx(i + 2) = x(i + 3 * M + 2);
dx(i + 3 * M ) = vx;
dx(i + 3 * M + 1 ) = vy;
dx(i + 3 * M + 2 ) = vz;
end
end
To perform a code, it's a good idea to analyze it with the Profiler:
https://es.mathworks.com/help/matlab/ref/profile.html
Anyway, I see some things you could change. First, avoid to draw in each iteration. Do it in the end or draw every M interations.
Secondly, avoid using a loop to define the graphic. Plot it with vector type input data.
Thirdly, ode45 can do the iterations for you and can return you a vector with all the interations. Use it to plot.
Some tips:
use tic and toc to measure the execution time of your code (lines, functions, etc.)
preassign arrays and avoid letting them grow in loops
do not use plot in a loop. Try to use low-level commands and change the value of exisiting lines by refering to their handles (like for example handle = line([0 1][2 3][4 5]) and then set(handle,'XDATA',[4 7],'YDATA',[2 6],'ZDATA',[2 4]))
MATLAB is an interpreted language, so your code will run slower in most cases compared to compiled programs
I have created a function in matlab which returns an image:
function [BW3] =lineAnalysis( BW1, O)
BW2 = double(BW1 > 0);
BW3 = BW2(2:end, :); % <-- REMOVE 1st LINE (ARTIFACT)!
if O==0; % 1 for left
BW3 = fliplr(BW3); % <-- FLIP IT HERE!
end
BW3_orig = BW3; % <-- SAVE ORIGINAL VERSION
% STEP 1
n = 10; t = 0.9;
c = 0:(n - 1);
j0 = 1;
while mean(mean(BW3(1 + c, j0 + c))) < t
j0 = j0 + 1;
end
% STEP 2
i0 = 1;
while mean(mean(BW3(i0 + c, 1 + c))) < t
i0 = i0 + 1;
end
% STEP 3
a = (1 - j0) / (i0 - 1);
b = j0 - a;
for i = 1:i0
for j = 1:round(a * i + b)
BW3(i, j) = 0;
end
end
% STEP 4
w = 20;
for i = 1:round(i0 - w / a)
for j = max(1, round(a * i + b + 1)):round(a * i + b + w)
BW3(i, j) = 1;
end
end
% PLOT RESULT
[BW3] = size(BW3);
[nr, nc] = size(BW3);
% figure
subplot(122), imagesc(BW3), colormap(gray)
axis equal off, axis([1, nc, 1, nr]), title('Corrected')
This works perfectly.
However I would like to return the result in my main program instead of executing the line
imshow(BW3);
within the function.
I have tried doing this:
[BW3]=lineAnalysis(P, O);
I then tried to show the image by doing
imshow(BW3);
However this doesnt work when I show the image like this. It simply diplays one black image and one white image.
Can anyone tell me how to fix this??
Thanks :)
The problem is near the end of your code:
function [BW3] =lineAnalysis( BW1, O)
%% your calculations here
% PLOT RESULT
[BW3] = size(BW3);
imshow(BW3);
In the line [BW3] = size(BW3); you are replacing the image with a 2 element vector that has the numbers of rows and columns of your image.
I have many points inside a square. I want to partition the square in many small rectangles and check how many points fall in each rectangle, i.e. I want to compute the joint probability distribution of the points. I am reporting a couple of common sense approaches, using loops and not very efficient:
% Data
N = 1e5; % number of points
xy = rand(N, 2); % coordinates of points
xy(randi(2*N, 100, 1)) = 0; % add some points on one side
xy(randi(2*N, 100, 1)) = 1; % add some points on the other side
xy(randi(N, 100, 1), :) = 0; % add some points on one corner
xy(randi(N, 100, 1), :) = 1; % add some points on one corner
inds= unique(randi(N, 100, 1)); xy(inds, :) = repmat([0 1], numel(inds), 1); % add some points on one corner
inds= unique(randi(N, 100, 1)); xy(inds, :) = repmat([1 0], numel(inds), 1); % add some points on one corner
% Intervals for rectangles
K1 = ceil(sqrt(N/5)); % number of intervals along x
K2 = K1; % number of intervals along y
int_x = [0:(1 / K1):1, 1+eps]; % intervals along x
int_y = [0:(1 / K2):1, 1+eps]; % intervals along y
% First approach
tic
count_cells = zeros(K1 + 1, K2 + 1);
for k1 = 1:K1+1
inds1 = (xy(:, 1) >= int_x(k1)) & (xy(:, 1) < int_x(k1 + 1));
for k2 = 1:K2+1
inds2 = (xy(:, 2) >= int_y(k2)) & (xy(:, 2) < int_y(k2 + 1));
count_cells(k1, k2) = sum(inds1 .* inds2);
end
end
toc
% Elapsed time is 46.090677 seconds.
% Second approach
tic
count_again = zeros(K1 + 2, K2 + 2);
for k1 = 1:K1+1
inds1 = (xy(:, 1) >= int_x(k1));
for k2 = 1:K2+1
inds2 = (xy(:, 2) >= int_y(k2));
count_again(k1, k2) = sum(inds1 .* inds2);
end
end
count_again_fix = diff(diff(count_again')');
toc
% Elapsed time is 22.903767 seconds.
% Check: the two solutions are equivalent
all(count_cells(:) == count_again_fix(:))
How can I do it more efficiently in terms of time, memory, and possibly avoiding loops?
EDIT --> I have just found this as well, it's the best solution found so far:
tic
count_cells_hist = hist3(xy, 'Edges', {int_x int_y});
count_cells_hist(end, :) = []; count_cells_hist(:, end) = [];
toc
all(count_cells(:) == count_cells_hist(:))
% Elapsed time is 0.245298 seconds.
but it requires the Statistics Toolbox.
EDIT --> Testing solution suggested by chappjc
tic
xcomps = single(bsxfun(#ge,xy(:,1),int_x));
ycomps = single(bsxfun(#ge,xy(:,2),int_y));
count_again = xcomps.' * ycomps; %' 143x143 = 143x1e5 * 1e5x143
count_again_fix = diff(diff(count_again')');
toc
% Elapsed time is 0.737546 seconds.
all(count_cells(:) == count_again_fix(:))
I have written a simple mex function which works very well when N is large. Of course it's cheating but still ...
The function is
#include "mex.h"
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
unsigned long int hh, ctrl; /* counters */
unsigned long int N, m, n; /* size of matrices */
unsigned long int *xy; /* data */
unsigned long int *count_cells; /* joint frequencies */
/* matrices needed */
mxArray *count_cellsArray;
/* Now we need to get the data */
if (nrhs == 3) {
xy = (unsigned long int*) mxGetData(prhs[0]);
N = (unsigned long int) mxGetM(prhs[0]);
m = (unsigned long int) mxGetScalar(prhs[1]);
n = (unsigned long int) mxGetScalar(prhs[2]);
}
/* Then build the matrices for the output */
count_cellsArray = mxCreateNumericMatrix(m + 1, n + 1, mxUINT32_CLASS, mxREAL);
count_cells = mxGetData(count_cellsArray);
plhs[0] = count_cellsArray;
hh = 0; /* counter for elements of xy */
/* for all points from 1 to N */
for(hh=0; hh<N; hh++) {
ctrl = (m + 1) * xy[N + hh] + xy[hh];
count_cells[ctrl] = count_cells[ctrl] + 1;
}
}
It can be saved in a file "joint_dist_points_2D.c", then compiled:
mex joint_dist_points_2D.c
And check it out:
% Data
N = 1e7; % number of points
xy = rand(N, 2); % coordinates of points
xy(randi(2*N, 1000, 1)) = 0; % add some points on one side
xy(randi(2*N, 1000, 1)) = 1; % add some points on the other side
xy(randi(N, 1000, 1), :) = 0; % add some points on one corner
xy(randi(N, 1000, 1), :) = 1; % add some points on one corner
inds= unique(randi(N, 1000, 1)); xy(inds, :) = repmat([0 1], numel(inds), 1); % add some points on one corner
inds= unique(randi(N, 1000, 1)); xy(inds, :) = repmat([1 0], numel(inds), 1); % add some points on one corner
% Intervals for rectangles
K1 = ceil(sqrt(N/5)); % number of intervals along x
K2 = ceil(sqrt(N/7)); % number of intervals along y
int_x = [0:(1 / K1):1, 1+eps]; % intervals along x
int_y = [0:(1 / K2):1, 1+eps]; % intervals along y
% Use Statistics Toolbox: hist3
tic
count_cells_hist = hist3(xy, 'Edges', {int_x int_y});
count_cells_hist(end, :) = []; count_cells_hist(:, end) = [];
toc
% Elapsed time is 4.414768 seconds.
% Use mex function
tic
xy2 = uint32(floor(xy ./ repmat([1 / K1, 1 / K2], N, 1)));
count_cells = joint_dist_points_2D(xy2, uint32(K1), uint32(K2));
toc
% Elapsed time is 0.586855 seconds.
% Check: the two solutions are equivalent
all(count_cells_hist(:) == count_cells(:))
Improving on code in question
Your loops (and the nested dot product) can be eliminated with bsxfun and matrix multiplication as follows:
xcomps = bsxfun(#ge,xy(:,1),int_x);
ycomps = bsxfun(#ge,xy(:,2),int_y);
count_again = double(xcomps).'*double(ycomps); %' 143x143 = 143x1e5 * 1e5x143
count_again_fix = diff(diff(count_again')');
The multiplication step accomplishes the AND and summation done in sum(inds1 .* inds2), but without looping over the density matrix. EDIT: If you use single instead of double, execution time is nearly halved, but be sure to convert your answer to double or whatever is required for the rest of the code. On my computer this takes around 0.5 sec.
Note: With rot90(count_again/size(xy,1),2) you have a CDF, and in rot90(count_again_fix/size(xy,1),2) you have a PDF.
Using accumarray
Another approach is to use accumarray to make the joint histogram after we bin the data.
Starting with int_x, int_y, K1, xy, etc.:
% take (0,1) data onto [1 K1], following A.Dondas approach for easy comparison
ii = floor(xy(:,1)*(K1-eps))+1; ii(ii<1) = 1; ii(ii>K1) = K1;
jj = floor(xy(:,2)*(K1-eps))+1; jj(jj<1) = 1; jj(jj>K1) = K1;
% create the histogram and normalize
H = accumarray([ii jj],ones(1,size(ii,1)));
PDF = H / size(xy,1); % for probabilities summing to 1
On my computer, this takes around 0.01 sec.
The output is the same as A.Donda's converted from sparse to full (full(H)). Although, as he A.Donda pointed out, it is correct to have the dimensions be K1xK1, rather than the size of count_again_fix in the OPs code that was K1+1xK1+1.
To get the CDF, I believe you can simply apply cumsum to each axis of the PDF.
chappjc's answer and using hist3 are all good, but since I happened to want to have something like this some time ago and for some reason didn't find hist3 I wrote it myself, and I thought I'd post it here as a bonus. It uses sparse to do the actual counting and returns the result as a sparse matrix, so it may be useful for dealing with a multimodal distribution where different modes are far apart – or for someone who doesn't have the Statistics Toolbox.
Application to francesco's data:
K1 = ceil(sqrt(N/5));
[H, xs, ys] = hist2d(xy(:, 1), xy(:, 2), [K1 K1], [0, 1 + eps, 0, 1 + eps]);
Called with output parameters the function just returns the result, without it makes a color plot.
Here's the function:
function [H, xs, ys] = hist2d(x, y, n, ax)
% plot 2d-histogram as an image
%
% hist2d(x, y, n, ax)
% [H, xs, ys] = hist2d(x, y, n, ax)
%
% x: data for horizontal axis
% y: data for vertical axis
% n: how many bins to use for each axis, default is [100 100]
% ax: axis limits for the plot, default is [min(x), max(x), min(y), max(y)]
% H: 2d-histogram as a sparse matrix, indices 1 & 2 correspond to x & y
% xs: corresponding vector of x-values
% ys: corresponding vector of y-values
%
% x and y have to be column vectors of the same size. Data points
% outside of the axis limits are allocated to the first or last bin,
% respectively. If output arguments are given, no plot is generated;
% it can be reproduced by "imagesc(ys, xs, H'); axis xy".
% defaults
if nargin < 3
n = [100 100];
end
if nargin < 4
ax = [min(x), max(x), min(y), max(y)];
end
% parameters
nx = n(1);
ny = n(2);
xl = ax(1 : 2);
yl = ax(3 : 4);
% generate histogram
i = floor((x - xl(1)) / diff(xl) * nx) + 1;
i(i < 1) = 1;
i(i > nx) = nx;
j = floor((y - yl(1)) / diff(yl) * ny) + 1;
j(j < 1) = 1;
j(j > ny) = ny;
H = sparse(i, j, ones(size(i)), nx, ny);
% generate axes
xs = (0.5 : nx) / nx * diff(xl) + xl(1);
ys = (0.5 : ny) / ny * diff(yl) + yl(1);
% possibly plot
if nargout == 0
imagesc(ys, xs, H')
axis xy
clear H xs ys
end
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.