Newer
Older
abgabensammlungSS15 / mr / ub8 / localize_jp / get_angle_bracket.m
@Jan-Peter Hohloch Jan-Peter Hohloch on 20 Jun 2015 509 bytes MR: A1
function [ p ] = get_angle_bracket( mean, cov )
%GET_ANGLE_BRACKET Compute points visualizing 95% orientation uncertainty

scale = 2; % in multiples of sigma
length = 0.5; % length of both arms of the "bracket"

p = zeros(2,3);

theta = mean(3);
sigmaTheta = sqrt(cov(3,3)); % marginalized standard deviation of theta
th1 = theta - scale*sigmaTheta;
th3 = theta + scale*sigmaTheta;

p(:,2) = mean(1:2);
p(:,1) = mean(1:2) + length*[cos(th1); sin(th1)];
p(:,3) = mean(1:2) + length*[cos(th3); sin(th3)];

end