sugr_ghm_update_E_Matrix

% GHM update params relative orientation/E-matrix

 estx = sugr_ghm_update_E_Matrix(estx,estx_r);

 estx = 3 x 4 matrix [b,R]
 estxr = 1 x 5 vector [db_r;dr]

 Wolfgang Förstner 09/2011
 wfoerstn@uni-bonn.de

0001 %% GHM update params relative orientation/E-matrix
0002 %
0003 % estx = sugr_ghm_update_E_Matrix(estx,estx_r);
0004 %
0005 % estx = 3 x 4 matrix [b,R]
0006 % estxr = 1 x 5 vector [db_r;dr]
0007 %
0008 % Wolfgang Förstner 09/2011
0009 % wfoerstn@uni-bonn.de
0010 
0011 function estx = sugr_ghm_update_E_Matrix(estx,estx_r)
0012 
0013 % initial values
0014 ba = estx(:,1);
0015 Ra = estx(:,2:4);
0016 
0017 % updates
0018 best = ba + null(ba') * estx_r(1:2);
0019 best = best / norm(best);
0020 Rest = expm(calc_S(estx_r(3:5)')) * Ra;
0021 
0022 % updated parameters
0023 estx = [best, Rest];