2012-06-05 13:15:26 +08:00
|
|
|
function covarianceEllipse3D(c,P)
|
|
|
|
|
% covarianceEllipse3D: plot a Gaussian as an uncertainty ellipse
|
|
|
|
|
% Based on Maybeck Vol 1, page 366
|
|
|
|
|
% k=2.296 corresponds to 1 std, 68.26% of all probability
|
|
|
|
|
% k=11.82 corresponds to 3 std, 99.74% of all probability
|
|
|
|
|
%
|
|
|
|
|
% Modified from http://www.mathworks.com/matlabcentral/newsreader/view_thread/42966
|
|
|
|
|
|
2012-06-06 07:51:12 +08:00
|
|
|
[e,s] = svd(P);
|
2012-06-05 13:15:26 +08:00
|
|
|
k = 11.82;
|
|
|
|
|
radii = k*sqrt(diag(s));
|
|
|
|
|
|
|
|
|
|
% generate data for "unrotated" ellipsoid
|
|
|
|
|
[xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3));
|
|
|
|
|
|
|
|
|
|
% rotate data with orientation matrix U and center M
|
|
|
|
|
data = kron(e(:,1),xc) + kron(e(:,2),yc) + kron(e(:,3),zc);
|
|
|
|
|
n = size(data,2);
|
2012-06-06 07:51:12 +08:00
|
|
|
x = data(1:n,:)+c(1);
|
|
|
|
|
y = data(n+1:2*n,:)+c(2);
|
|
|
|
|
z = data(2*n+1:end,:)+c(3);
|
2012-06-05 13:15:26 +08:00
|
|
|
|
|
|
|
|
% now plot the rotated ellipse
|
2012-06-06 07:51:12 +08:00
|
|
|
sc = mesh(x,y,z,abs(xc));
|
2012-06-05 13:15:26 +08:00
|
|
|
shading interp
|
|
|
|
|
alpha(0.5)
|
|
|
|
|
axis equal
|
