Code covered by the BSD License

# frontal

### Felipe G. Nievinski (view profile)

15 Mar 2011 (Updated )

Do: C = frontal_mtimes(A, b); not: for k=1:size(A,3), C(:,:,k) = A(:,:,k) * b(:,:,k); end

frontal_propagate_cov (cov_in, jacobian)
```function [cov_out, std_out] = frontal_propagate_cov (cov_in, jacobian)
if isscalar(jacobian) && abs(jacobian) == 1
cov_out = cov_in;
if (nargout > 1)
temp = defrontal_pt(frontal_transpose(frontal_diag(cov_out)));
std_out = sqrt(temp);
end
return;
end

n = size(jacobian, 3);
myassert ( size(cov_in, 3) == n );
myassert ( size(cov_in, 1) == 3 && size(cov_in, 2) == 3 );
%myassert ( isequal(size(cov_in), [3, 3, n]) );  % doesn't work

%% simplified expression for a single point:
%cov_out = jacobian * cov_in * jacobian.';
%std_out = sqrt(diag(cov_out)).';

cov_out = frontal_mtimes(frontal_mtimes(...
jacobian, cov_in), frontal_transpose(jacobian));
if (nargout > 1)
temp = defrontal_pt(frontal_transpose(frontal_diag(cov_out)));
std_out = sqrt(temp);
end
%jacobian = frontal(jacobian);
%cov_in = frontal(cov_in);
%cov_out = jacobian * cov_in * jacobian.';
%if (nargout > 1)
%    temp = defrontal_pt(defrontal(diag(cov_out).'));
%    std_out = sqrt(temp);
%end
%cov_out = defrontal(cov_out);
end

%!test
%! jacobian = eye(3);
%! cov_in = rand(3,3);
%! cov_out = frontal_propagate_cov (cov_in, jacobian);
%! myassert (cov_out, cov_in, -eps);

%!test
%! % Multiple points:
%! n = 10000;%ceil(10*rand);
%! cov_in = repmat(rand(3,3), [1,1,n]);
%! jacobian = repmat(eye(3,3), [1,1,n]);
%!
%! [cov_out, std_out] = ...
%!     frontal_propagate_cov (cov_in, jacobian);
%!
%! myassert ( size(cov_out,3) == n )
%! myassert ( size(std_out,1) == n )
%! myassert ( cov_in, repmat(cov_in(:,:,1), [1,1,n]))
%! % since jacobian is identity matrix:
%! myassert ( cov_in, cov_out )

%!test
%! cov_in = rand;
%! cov_out = frontal_propagate_cov (cov_in, 1);
%!   myassert (cov_out, cov_in);
%! cov_out = frontal_propagate_cov (cov_in, -1);
%!   myassert (cov_out, cov_in);
%! [cov_out, std_out] = frontal_propagate_cov (cov_in, -1);

```