0001 function est_x = finalize_sym_lsm(...
0002 iter,...
0003 xa,...
0004 est_xa,...
0005 plot_type)
0006
0007
0008
0009
0010
0011 N_iter=iter ;
0012
0013 Tg_esta.x = [reshape(xa(1:6),2,3);[0 0 1]];
0014 A_est = Tg_esta.x*Tg_esta.x;
0015 Tr_esta.x = [xa(7:8)';0 1];
0016 R_est = Tr_esta.x*Tr_esta.x;
0017
0018
0019 a = est_xa.x;
0020 J = [2*a(1) a(3) a(2) 0 0 0 0 0;
0021 a(2) a(1)+a(4) 0 a(2) 0 0 0 0;
0022 a(3) 0 a(1)+a(4) a(3) 0 0 0 0;
0023 0 a(3) a(2) 2*a(4) 0 0 0 0;
0024 a(5) 0 a(6) 0 a(1)+1 a(3) 0 0;
0025 0 a(5) 0 a(6) a(2) a(4)+1 0 0;
0026 0 0 0 0 0 0 2*a(7) 0;
0027 0 0 0 0 0 0 a(8) 1+a(7) ];
0028 C_yy = J * est_xa.C * J';
0029 if plot_type > 0
0030 disp('=== Final result =============================================')
0031 end
0032 if plot_type > 1
0033 Sigma_rho = covariance_2_sigma_rho(C_yy)
0034 end
0035
0036
0037 est_x.x = [reshape(A_est(1:2,:),6,1);R_est(1,:)'];
0038 est_x.C = C_yy;
0039 if plot_type > 1
0040 sigmas = diag(sqrt(C_yy))'
0041 A_est = A_est(1:3,:)
0042 Sigmas_A = reshape(sigmas(1:6),2,3)
0043 R_est = R_est(1,1:2)
0044 Sigmas_R = sigmas(7:8)
0045 end
0046 est_x.Ah = A_est;
0047 est_x.R = R_est;
0048
0049 end
0050