├── lidarData.mat ├── README.md ├── timeUpdate.m ├── drawEllipse.m ├── convertMultiEllipseTracktoGM.m ├── CreateInitialStatistics.m ├── LoadParameters.m ├── main.m └── MultiEllipseVB.m /lidarData.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Metu-Sensor-Fusion-Lab/Multi-Ellipsoidal-Extended-Target-Tracking-with-Variational-Bayes-Inference/HEAD/lidarData.mat -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | This repository presents an example implementation of the algorithm proposed in the following paper. 2 | 3 | "Multi-Ellipsoidal Extended Target Tracking with Variational Bayes Inference" by B. Tuncer, E. Özkan, and U. Orguner 4 | URL: https://www.techrxiv.org/articles/preprint/Multi-Ellipsoidal_Extended_Target_Tracking_with_Variational_Bayes_Inference/14178494 5 | 6 | -------------------------------------------------------------------------------- /timeUpdate.m: -------------------------------------------------------------------------------- 1 | function track=timeUpdate(track,model,algorithm) 2 | 3 | ny=model.measurement.n; 4 | F=model.state.F; 5 | Q=model.state.Q; 6 | lambda=algorithm.VB.lambda; 7 | 8 | track.x=F*track.x; 9 | track.P=F*track.P*F'+Q; 10 | track.v=max(lambda*track.v,2*ny+1);%avoids v falling below the minimum required for IW density 11 | track.V=lambda*track.V; 12 | track.a=lambda*track.a; 13 | -------------------------------------------------------------------------------- /drawEllipse.m: -------------------------------------------------------------------------------- 1 | function h=drawEllipse(x,P,c,ax,linewidth,color) 2 | 3 | [nx,N]=size(x); 4 | if nx~=2 5 | error('nx must be 2'); 6 | end 7 | phi=linspace(0,2*pi,100); 8 | z=[cos(phi);sin(phi)]; 9 | 10 | h=zeros(1,N); 11 | htext=zeros(1,N); 12 | if (N==1) 13 | [U,S,V]=svd(P); 14 | Sroot=sqrt(S); 15 | Ph=U*Sroot; 16 | h=plot(x(1)+c*Ph(1,:)*z,x(2)+c*Ph(2,:)*z,['-'],'Color',color,'parent',ax,'linewidth',linewidth); 17 | end -------------------------------------------------------------------------------- /convertMultiEllipseTracktoGM.m: -------------------------------------------------------------------------------- 1 | function [GM, meanTarget, covTarget]=convertMultiEllipseTracktoGM(stats) 2 | 3 | ny=size(stats.V,1); 4 | L=length(stats.x)/ny-1; 5 | for i=1:L 6 | if i==1 7 | GM.x(:,1)=stats.x(1:ny); 8 | else 9 | GM.x(:,i)=stats.x(1:ny)+stats.x(ny*i+1:ny*i+ny); 10 | end 11 | GM.P(:,:,i)=stats.V(:,:,i)/(stats.v(i)-2*ny-2); 12 | end 13 | 14 | GM.pi=stats.a/sum(stats.a); 15 | 16 | meanTarget=GM.x*GM.pi'; 17 | covTarget=zeros(ny); 18 | for i=1:L 19 | covTarget=covTarget+GM.pi(i)*(GM.P(:,:,i)+(GM.x(:,i)-meanTarget)*(GM.x(:,i)-meanTarget)'); 20 | end -------------------------------------------------------------------------------- /CreateInitialStatistics.m: -------------------------------------------------------------------------------- 1 | function initstats=CreateInitialStatistics(Y,algorithm) 2 | %Using Multi Ellipse VB 3 | 4 | ny=size(Y,1); 5 | Nmeas=size(Y,2); 6 | 7 | s=algorithm.VB.s; 8 | L=algorithm.VB.L; 9 | 10 | meanY=mean(Y,2); 11 | if Nmeas<=ny 12 | covY=1e-2*eye(ny); 13 | else 14 | covY=cov(Y')/s; 15 | end 16 | 17 | mu=zeros(ny,L);%initial means 18 | sigma=repmat(covY,1,1,L);%initial covariances 19 | piVB=ones(1,L)/L;%equal initial probabilities 20 | 21 | [V, D]=eig(covY); 22 | [maxEig, maxind]=max(sqrt(diag(D))); 23 | maxVec=V(:,maxind); 24 | h=2*maxEig/L;%distance between initial means 25 | if mod(L,2) 26 | mu(:,1)=meanY; 27 | ind=1; 28 | for i=1:(L-1)/2 29 | ind=ind+1; 30 | mu(:,ind)=meanY+i*h*maxVec; 31 | ind=ind+1; 32 | mu(:,ind)=meanY-i*h*maxVec; 33 | end 34 | else 35 | ind=0; 36 | for i=1:L/2 37 | ind=ind+1; 38 | mu(:,ind)=meanY+(2*i-1)*h*maxVec/2; 39 | ind=ind+1; 40 | mu(:,ind)=meanY-(2*i-1)*h*maxVec/2; 41 | end 42 | end 43 | initstats.a=algorithm.Prior.a; 44 | initstats.x = [mu(:,1); zeros(ny,1)]; 45 | initstats.P = zeros(4); 46 | for ell = 2:L 47 | initstats.x = [initstats.x; mu(:,ell)-mu(:,1)]; 48 | initstats.P = blkdiag(initstats.P,zeros(2)); 49 | end 50 | initstats.v=algorithm.Prior.v; 51 | for ell=1:L 52 | initstats.V(:,:,ell)=(algorithm.Prior.v(ell)-2*ny-2)*(sigma(:,:,ell)/L^2); 53 | end 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /LoadParameters.m: -------------------------------------------------------------------------------- 1 | L=2; %number of ellipsoidal extent components 2 | 3 | T=0.1; 4 | model.state.T=T; 5 | 6 | ny=2; 7 | model.measurement.n=ny;%measurement dimension 8 | Zny=zeros(ny); 9 | Iny=eye(ny); 10 | model.measurement.R=0.001*Iny;%measurement noise covariance 11 | model.measurement.s=1/4;%s pararameter 12 | model.measurement.gamma=30;%Average Number of Measurements of Extended Targets 13 | 14 | tempF=eye(ny);%F for non-center components 15 | F=[Iny T*Iny; Zny Iny]; 16 | sigmaQ=1; 17 | tempQ=eye(ny);%Q for non-center components 18 | Q=sigmaQ^2*[T^3/3*Iny T^2/2*Iny; T^2/2*Iny T*Iny]; 19 | model.measurement.H=cell(1,L); 20 | 21 | model.state.F=F; 22 | model.state.Q=Q; 23 | for ell=1:L-1 24 | model.state.F=blkdiag(model.state.F,tempF); 25 | model.state.Q=blkdiag(model.state.Q,tempQ); 26 | end 27 | model.measurement.H=repmat([Iny repmat(Zny,1,L)],1,1,L); 28 | for l = 2:L 29 | model.measurement.H(1,2*l+1,l) = 1; 30 | model.measurement.H(2,2*l+2,l) = 1; 31 | end 32 | 33 | P0prior=blkdiag(10000*Iny,10000*Iny); %Prior covariance 34 | vprior=10;%Prior v value 35 | Vprior=10*Iny;%Prior V value 36 | aprior=1;%Prior a value 37 | Pe=100^2*eye(2); %Prior covariance of mixture component positions from the center 38 | 39 | algorithm.Prior.a=aprior*ones(1,L); 40 | algorithm.Prior.x=zeros(4,1); %prior target state is zero 41 | algorithm.Prior.P=P0prior; 42 | algorithm.Prior.v=vprior*ones(1,L); 43 | algorithm.Prior.V=repmat(Vprior,1,1,L); 44 | for ell = 2:L 45 | algorithm.Prior.x = [algorithm.Prior.x; zeros(ny,1)]; 46 | algorithm.Prior.P = blkdiag(algorithm.Prior.P,Pe); 47 | end 48 | 49 | %Algorithm Parameters 50 | algorithm.VB.lambda=0.9; %time update forgetting factor 51 | algorithm.VB.maxNiter=30; %Maximum number of iterations in VB 52 | algorithm.VB.convergenceThreshold=1e-3; %convergence threshold in VB 53 | algorithm.VB.L=L;%number of ellipsoid components 54 | algorithm.VB.s=1/4;%s value for VB 55 | -------------------------------------------------------------------------------- /main.m: -------------------------------------------------------------------------------- 1 | %********************************************************************* 2 | %** Implementation of the Multi-Ellipsoidal VB algorithm ** 3 | %** based on the article ** 4 | %** "Multi-Ellipsoidal Extended Target Tracking with ** 5 | %** Variational Bayes Inference" ** 6 | %** This article is accepted to IEEE-TSP. ** 7 | %** Barkın Tuncer, Umut Orguner, and Emre Özkan ** 8 | %** Further information: ** 9 | %** https://github.com/Metu-Sensor-Fusion-Lab ** 10 | %** http://sensorfusion.eee.metu.edu.tr/ ** 11 | %** ** 12 | %********************************************************************* 13 | 14 | close all 15 | clear 16 | 17 | load lidarData.mat %load Lidar Data (pointMeasurements2D) 18 | 19 | LoadParameters;%Load scenario parameters 20 | 21 | timeIndex=0; 22 | for t=0:T:4.9 23 | a=find(pointMeasurements2D(1:end,1)>=t & pointMeasurements2D(1:end,1)<=t+T); 24 | Y=[pointMeasurements2D(a,2)';pointMeasurements2D(a,3)']; %current measurements 25 | if ~isempty(Y) 26 | if timeIndex==0 27 | initstats=CreateInitialStatistics(Y,algorithm); %Create initial statistics based on the covariance of the measurements and the selected number of ellipsoidal extent components 28 | [track, ~] = MultiEllipseVB(Y,algorithm.Prior,initstats,model.measurement,algorithm.VB); %measurement update 29 | else 30 | [track, ~] = MultiEllipseVB(Y,track_,[],model.measurement,algorithm.VB); %measurement update 31 | end 32 | else 33 | track=track_; 34 | end 35 | track_=timeUpdate(track,model,algorithm);%time update 36 | initialGM=convertMultiEllipseTracktoGM(initstats);%convert initial GIW mixture to GM 37 | GMtrack=convertMultiEllipseTracktoGM(track);%convert GIW mixture to GM 38 | 39 | figure(1); 40 | plot(pointMeasurements2D(a,2),pointMeasurements2D(a,3), '+b') 41 | title(['time =' num2str(t)]); 42 | xlim([-20, 10 ]), ylim([-30, 0 ]), grid on, 43 | hold on 44 | L=size(GMtrack.x,2); 45 | title('All Measurements') 46 | 47 | figure(2); 48 | for ell=1:L 49 | plot(GMtrack.x(1,ell),GMtrack.x(2,ell),'bx') 50 | hold on 51 | drawEllipse(GMtrack.x(1:2,ell),GMtrack.P(:,:,ell),1,gca,1.5,[0, 0.4470, 0.7410]); 52 | end 53 | title('All Estimates') 54 | 55 | h=figure(3);%Draw results at every 10 sampling instants. Draw all components. 56 | if mod(timeIndex,10)==0 57 | plot(pointMeasurements2D(a,2),pointMeasurements2D(a,3), '.r') 58 | hold on 59 | if timeIndex==0%draw initial ellipses 60 | for ell=1:L 61 | plot(initialGM.x(1,ell),initialGM.x(2,ell),'Color',[0.9290, 0.6940, 0.1250],'Marker','x') 62 | drawEllipse(initialGM.x(1:2,ell),initialGM.P(:,:,ell),1,gca,1.5,[0.9290, 0.6940, 0.1250]); 63 | end 64 | end 65 | for ell=1:L %draw filtered ellipses 66 | plot(GMtrack.x(1,ell),GMtrack.x(2,ell),'Color',[0, 0.4470, 0.7410],'Marker','x') 67 | drawEllipse(GMtrack.x(1:2,ell),GMtrack.P(:,:,ell),1,gca,1.5,[0, 0.4470, 0.7410]); 68 | end 69 | end 70 | xlabel('x [m]','Interpreter','Latex') 71 | ylabel('y [m]','Interpreter','Latex') 72 | title('Estimates Every 10 Sampling Times') 73 | grid on 74 | 75 | 76 | figure(4);%Draw results at every 10 sampling instants. Draw only high weighted components (Automatic Model Order Selection if L>2) 77 | if mod(timeIndex,10)==0 78 | plot(pointMeasurements2D(a,2),pointMeasurements2D(a,3), '.r') 79 | hold on 80 | if timeIndex==0%draw initial ellipses 81 | for ell=1:L 82 | plot(initialGM.x(1,ell),initialGM.x(2,ell),'Color',[0.9290, 0.6940, 0.1250],'Marker','x') 83 | drawEllipse(initialGM.x(1:2,ell),initialGM.P(:,:,ell),1,gca,1.5,[0.9290, 0.6940, 0.1250]); 84 | end 85 | end 86 | for ell=1:L %draw filtered ellipses 87 | if GMtrack.pi(ell)>0.01 %Draw only high weight components 88 | plot(GMtrack.x(1,ell),GMtrack.x(2,ell),'Color',[0, 0.4470, 0.7410],'Marker','x') 89 | drawEllipse(GMtrack.x(1:2,ell),GMtrack.P(:,:,ell),1,gca,1.5,[0, 0.4470, 0.7410]); 90 | end 91 | end 92 | end 93 | xlabel('x [m]','Interpreter','Latex') 94 | ylabel('y [m]','Interpreter','Latex') 95 | title('Estimates Every 10 Sampling Times') 96 | grid on 97 | pause(0.1) 98 | timeIndex=timeIndex+1; 99 | end 100 | 101 | %set(h,'Units','Inches'); 102 | %pos = get(h,'Position'); 103 | %set(h,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)]) 104 | %print(h,'LidarFigure','-dpdf','-r0') 105 | -------------------------------------------------------------------------------- /MultiEllipseVB.m: -------------------------------------------------------------------------------- 1 | function [statsout, logpyy] = MultiEllipseVB(Y,stats,initstats,model,algorithm) 2 | % Implementation of the measurement update of 3 | % "Multi-Ellipsoidal Extended Target Tracking with Variational Bayes Inference" 4 | % By Barkın Tuncer, Umut Orguner, Emre Özkan 5 | 6 | % INPUTS 7 | % Y: Measurements (2 x Ny double) (Ny is the number of measurements) 8 | 9 | % stats: Predicted Statistics (structure) 10 | % stats.x: Predicted kinematic mean (2*L+1 x 1 double) 11 | % stats.P: Predicted kinematic covariance (2*L+1 x 2*L+1 double) 12 | % stats.v: Predicted extent dof (1xL double) 13 | % stats.V: Predicted extent scale matrices (ny x ny x L double) 14 | % stats.a: Predicted Dirichlet parameters (1xL double) 15 | 16 | % initstats: Initial Statistics (structure) (Optional. Set [] if not necessary. When set [] the VB iterations are started based on predicted statistics (i.e., stats)) 17 | % initstats.x: Initial kinematic mean (2*L+1 x 1 double) 18 | % initstats.P: Initial kinematic covariance (2*L+1 x 2*L+1 double) 19 | % initstats.v: Initial extent dof (1xL double) 20 | % initstats.V: Initial extent scale matrices (ny x ny x L double) 21 | % initstats.a: Initial Dirichlet parameters (1xL double) 22 | 23 | % model: Measurement Model Parameters (structure) 24 | % model.N: Measurement dimension ny (1 x 1 integer) 25 | % model.R: Measurement noise Covariance (ny x ny double) 26 | % model.H: Measurement matrices for extent components (ny x 2*L+1 x L double) 27 | % model.s: Measurement scale parameter (1 x 1 double) 28 | 29 | % algorithm: Algorithm Parameters (structure) 30 | % algorithm.L=L: Number of extent ellipsoids (1 x 1 integer) 31 | % algorithm.convergenceThreshold: Convergence threshold in VB (1 x 1 double) 32 | % algorithm.maxNiter: Maximum number of iterations in VB (1 x 1 integer) 33 | 34 | % OUTPUTS 35 | % statsout: Updated Statistics (structure) 36 | % statsout.x: Updated kinematic mean (2*L+1 x 1 double) 37 | % statsout.P: Updated kinematic covariance (2*L+1 x 2*L+1 double) 38 | % statsout.v: Updated extent dof (1xL double) 39 | % statsout.V: Updated extent scale matrices (ny x ny x L double) 40 | % statsout.a: Updated Dirichlet parameters (1xL double) 41 | 42 | % logpyy: Predictive likelihood (1 x 1 double) 43 | 44 | ny=model.n; 45 | s=model.s; 46 | R=model.R; 47 | H=model.H; 48 | 49 | L=algorithm.L; 50 | maxNiter=algorithm.maxNiter; 51 | convergenceThreshold=algorithm.convergenceThreshold; 52 | 53 | statsout=stats; 54 | 55 | %Predicted quantities 56 | x = stats.x; 57 | P = stats.P; 58 | v = stats.v; 59 | V = stats.V; 60 | a = stats.a; 61 | 62 | nx = length(x); 63 | 64 | if isempty(initstats) 65 | %initialize VB iteration 66 | xkk_ = stats.x; 67 | %Pkk_= stats.P; 68 | Pkk_= zeros(nx); 69 | vkk_ = stats.v; 70 | Vkk_ = stats.V; 71 | akk_ = stats.a; 72 | else 73 | xkk_ = initstats.x; 74 | Pkk_= initstats.P; 75 | vkk_ = initstats.v; 76 | Vkk_ = initstats.V; 77 | akk_ = initstats.a; 78 | end 79 | zkk_ = Y; 80 | Nmeas = size(Y,2); 81 | Pzkk_ = repmat(zeros(ny),1,1,Nmeas); 82 | 83 | %Allocate 84 | xkk = xkk_; 85 | Pkk = Pkk_; 86 | vkk = vkk_; 87 | Vkk = Vkk_; 88 | akk = akk_; 89 | zkk = zkk_; 90 | Pzkk = Pzkk_; 91 | 92 | loggammakk = zeros(Nmeas,L); 93 | invsX=zeros(ny,ny,L); 94 | invsXHxkk_=zeros(ny,L); 95 | Hxkk_=zeros(ny,L); 96 | HPkk_H=zeros(ny,ny,L); 97 | Wjl=zeros(ny,ny,Nmeas,L); 98 | 99 | invR=inv(R);%R^-1 100 | invP=inv(P);% inv(P_k|k-1) 101 | 102 | iter=0; 103 | convergenceStat=inf; 104 | 105 | plotFlag=0; % For debugging purposes, make this 1, then the extent ellipsoids will be drawn at each VB iteration. 106 | if plotFlag %Plot Extent Ellipsoids with Measurements (For debugging purposes) 107 | plotstats.x=xkk; 108 | plotstats.P=Pkk; 109 | plotstats.v=vkk; 110 | plotstats.V=Vkk; 111 | plotstats.a=akk; 112 | 113 | figure(547) 114 | plot(Y(1,:),Y(2,:),'.') 115 | hold on 116 | plot(zkk(1,:),zkk(2,:),'r.') 117 | 118 | [GM, ~, ~]=convertMultiEllipseTracktoGM(plotstats); 119 | for ell=1:L 120 | drawEllipse(GM.x(:,ell),GM.P(:,:,ell),1,gca,1.5,[0, 0.4470, 0.7410]); 121 | end 122 | title(['Iteration No:', num2str(iter)]) 123 | pause(0.5) 124 | cla 125 | end 126 | 127 | while iterconvergenceThreshold 128 | iter=iter+1; 129 | oldxkk=xkk; 130 | for ell=1:L%Do these calculations only once in each iteration. Not as many as the number of measurements in the double for loop below 131 | invsX(:,:,ell)=(vkk_(ell) - ny - 1)*inv(s*Vkk_(:,:,ell)); %Find E[inv(sXk)] only once 132 | invsXHxkk_(:,ell) = invsX(:,:,ell) * H(:,:,ell) * xkk_; 133 | Hxkk_(:,ell)=H(:,:,ell)*xkk_; 134 | HPkk_H(:,:,ell)=H(:,:,ell) * Pkk_ * H(:,:,ell)'; 135 | temploggammatildekk=psi(akk_(ell)) - psi(sum(akk_))- 0.5 * log(det(Vkk_(:,:,ell))) + 0.5*ny*log(2) + 0.5 *sum(psi((vkk_(ell) - ny - [1:ny])/2)); 136 | for j=1:Nmeas 137 | Wjl(:,:,j,ell) = (zkk_(:,j) - Hxkk_(:,ell)) * (zkk_(:,j) - Hxkk_(:,ell))'+ Pzkk_(:,:,j)+ HPkk_H(:,:,ell);%22 138 | %loggammakk(j,ell) = temploggammatildekk - 0.5*trace(invsX(:,:,ell)* Wjl(:,:,j,ell));%23 139 | loggammakk(j,ell) = temploggammatildekk - 0.5*(invsX(1,:,ell)*Wjl(:,1,j,ell)+invsX(2,:,ell)*Wjl(:,2,j,ell));%23 140 | end 141 | end 142 | for j=1:Nmeas 143 | loggammakk(j,:)=loggammakk(j,:)-logSum(loggammakk(j,:));%Normalize 144 | end 145 | gammakk = exp(loggammakk);%26 146 | 147 | vkk = v + sum(gammakk,1);%34 148 | akk = a + sum(gammakk,1);%36 149 | Vkk=V; %Set Vkk to predicted V initially 150 | for j = 1 : Nmeas 151 | tmpCov = zeros(ny); 152 | for ell = 1 : L 153 | tmpCov = tmpCov + gammakk(j,ell) * invsX(:,:,ell);%30 154 | Vkk(:,:,ell)=Vkk(:,:,ell)+(gammakk(j,ell)/s)*Wjl(:,:,j,ell);%35 155 | end 156 | Pzkk(:,:,j) = inv(invR + tmpCov);%30 157 | zkk(:,j) = Pzkk(:,:,j) * (invR * Y(:,j) + invsXHxkk_*gammakk(j,:)');%31 158 | end 159 | 160 | invPkk=invP;% Set posterior information matrix to prior information matrix initially 161 | invPkkxkk=invP*x;% Set posterior information vector to prior information vector initially 162 | for ell=1:L 163 | invPkkxkk=invPkkxkk+H(:,:,ell)'*invsX(:,:,ell)*zkk_*gammakk(:,ell);%42 164 | invPkk=invPkk+sum(gammakk(:,ell))*H(:,:,ell)'*invsX(:,:,ell)*H(:,:,ell);%43 165 | end 166 | Pkk=inv(invPkk); 167 | xkk=Pkk*invPkkxkk; 168 | 169 | convergenceStat=max(max(abs(xkk-oldxkk))); 170 | 171 | xkk_ = xkk; 172 | Pkk_ = Pkk; 173 | Vkk_ = Vkk; 174 | vkk_ = vkk; 175 | zkk_ = zkk; 176 | Pzkk_ = Pzkk; 177 | akk_ = akk; 178 | 179 | 180 | if plotFlag %Plot Extent Ellipsoids with Measurements (For debugging purposes) 181 | plotstats.x=xkk; 182 | plotstats.P=Pkk; 183 | plotstats.v=vkk; 184 | plotstats.V=Vkk; 185 | plotstats.a=akk; 186 | 187 | figure(547) 188 | plot(Y(1,:),Y(2,:),'.') 189 | hold on 190 | plot(zkk(1,:),zkk(2,:),'r.') 191 | 192 | [GM, ~, ~]=convertMultiEllipseTracktoGM(plotstats); 193 | for ell=1:L 194 | drawEllipse(GM.x(:,ell),GM.P(:,:,ell),1,gca,1.5,[0, 0.4470, 0.7410]); 195 | end 196 | title(['Iteration No:', num2str(iter)]) 197 | pause(0.5) 198 | cla 199 | end 200 | end 201 | 202 | 203 | statsout.x=xkk; 204 | statsout.P=Pkk; 205 | statsout.v=vkk; 206 | statsout.V=Vkk; 207 | statsout.a=akk; 208 | 209 | %Calculate the predictive likelihood 210 | logpyy=sum(logmvnpdf((Y-zkk),zeros(2,1),R)); 211 | for j=1:Nmeas 212 | logpyy=logpyy+0.5*log(det(2*Pzkk(:,:,j)/s))-0.5*(trace(invR*Pzkk(:,:,j))-ny); 213 | end 214 | for ell=1:L 215 | logpyy=logpyy-0.5*(vkk(ell)-ny-1)*log(det(Vkk(:,:,ell)))+0.5*(v(ell)-ny-1)*log(det(V(:,:,ell))); 216 | logpyy=logpyy+logGammad(0.5*(vkk(ell)-ny-1),ny)-logGammad(0.5*(v(ell)-ny-1),ny); 217 | end 218 | logpyy=logpyy-0.5*(trace(Pkk*invP)-nx)+0.5*log(det(2*pi*Pkk))+logmvnpdf(xkk,x,P); 219 | logpyy=logpyy-sum(sum(exp(loggammakk).*loggammakk)); 220 | suma=sum(a); 221 | sumakk=sum(akk); 222 | logpyy=logpyy+gammaln(suma)-sum(gammaln(a))-gammaln(sumakk)+sum(gammaln(akk)); 223 | 224 | function outlogGammad=logGammad(x,d) 225 | D=1:d; 226 | outlogGammad=0.25*d*(d-1)*log(pi)+sum(gammaln(x-0.5*(D-1))); 227 | 228 | function out=logmvnpdf(y,x,P) 229 | Ny=size(y,2); 230 | difference=y-repmat(x,1,Ny); 231 | out=-0.5*log(det(2*pi*P))-0.5*sum(difference'.*(difference'/P),2); 232 | 233 | function [logOfSum] = logSum(logA) 234 | 235 | N=size(logA,1); 236 | 237 | maxLogA = max(logA); 238 | logOfSum = maxLogA+log(sum(exp(logA-repmat(maxLogA,N,1)))); 239 | --------------------------------------------------------------------------------