% Gauss Helmert Model for Groups
g_i(^l_i,^x) = 0 group of constraints
all observational groups have the same dimension d_I
all observational groups are mutually uncorrelated
all groups of constraints have the same size d_G
constraints must not overlap, ie must not use the same observation
covariance matrix of estimates is assumed to be full
Only one vector of unknowns
no robust estimation
lm = I x d_I matrix of observational groups
Cov_ll_m = I x d_i^2 matrix of vectorized covariance matrices
cg_f = d_G x 1 constraint function -> cg, A, B
xa = Ux1 vector of approximate parameters
ux = function to update parameters x = ux(xa,dx)
sx = Ux1 vector of standard deviations of parameters
Tx = threshold for convergence
maxiter = maximum number of iterations
rU = range of unknown parameters of interest
est_x = Ux1 estimated parameter
Cov_xx = UxU theoretical covariance matrix of estimated parameters
may be full
sigma_0q = estimated variance factor (=1 if R=0)
R = redundancy
vv = Ixd_I vector of estimated corrections
Xv = Ix1 vector of Xhi^2-test statistic (using sigma_0=1)
Rim = I x d_I^2 redundancy matrices as row vectors
nabla_lv = Ix1 minimum detectabele outlier
muv = Ix1 vector of senstivity factors wrt to all parameters
muv1 = Ix1 vector of senstivity factors wrt to selected group
Uv1q = I x d_I^2 matrices of contributions to selected group
Uv2 = I x d_I^2 matrices of contribution to remaining parameters
[est_x,Cov_xx,sigma_0q,R,vv,Xv,Rim,nabla_lv,muv,muv1,uv1q,uv2]=...
GaussHelmertModelGroups(lm,Cov_ll_m,cg_f,xa,ux,sx,Tx,maxiter,rU)
Wolfgang Förstner 2016-06-06
wfoerstn@uni-bonn.de0001 %% Gauss Helmert Model for Groups 0002 % 0003 % g_i(^l_i,^x) = 0 group of constraints 0004 % 0005 % all observational groups have the same dimension d_I 0006 % all observational groups are mutually uncorrelated 0007 % all groups of constraints have the same size d_G 0008 % constraints must not overlap, ie must not use the same observation 0009 % covariance matrix of estimates is assumed to be full 0010 % Only one vector of unknowns 0011 % no robust estimation 0012 % 0013 % lm = I x d_I matrix of observational groups 0014 % Cov_ll_m = I x d_i^2 matrix of vectorized covariance matrices 0015 % cg_f = d_G x 1 constraint function -> cg, A, B 0016 % xa = Ux1 vector of approximate parameters 0017 % ux = function to update parameters x = ux(xa,dx) 0018 % sx = Ux1 vector of standard deviations of parameters 0019 % Tx = threshold for convergence 0020 % maxiter = maximum number of iterations 0021 % rU = range of unknown parameters of interest 0022 % 0023 % est_x = Ux1 estimated parameter 0024 % Cov_xx = UxU theoretical covariance matrix of estimated parameters 0025 % may be full 0026 % sigma_0q = estimated variance factor (=1 if R=0) 0027 % R = redundancy 0028 % vv = Ixd_I vector of estimated corrections 0029 % Xv = Ix1 vector of Xhi^2-test statistic (using sigma_0=1) 0030 % Rim = I x d_I^2 redundancy matrices as row vectors 0031 % nabla_lv = Ix1 minimum detectabele outlier 0032 % muv = Ix1 vector of senstivity factors wrt to all parameters 0033 % muv1 = Ix1 vector of senstivity factors wrt to selected group 0034 % Uv1q = I x d_I^2 matrices of contributions to selected group 0035 % Uv2 = I x d_I^2 matrices of contribution to remaining parameters 0036 % 0037 % [est_x,Cov_xx,sigma_0q,R,vv,Xv,Rim,nabla_lv,muv,muv1,uv1q,uv2]=... 0038 % GaussHelmertModelGroups(lm,Cov_ll_m,cg_f,xa,ux,sx,Tx,maxiter,rU) 0039 % 0040 % Wolfgang Förstner 2016-06-06 0041 % wfoerstn@uni-bonn.de 0042 0043 function [est_x,Cov_xx,sigma_0q,R,vv,Xv,Rim,nabla_lv,muv,muv1,Uv1q,Uv2]=... 0044 GaussHelmertModelGroups(lm,Cov_ll_m,cg_f,xa,ux,sx,Tx,maxiter,rU) 0045 0046 %% initialization --------------------------------------------------------- 0047 % numbers N, G, and U 0048 % numbers I, d_I 0049 [I,d_I] = size(lm); 0050 [cg,Am,Bm] = cg_f(lm(1,:)',lm(1,:)',xa); 0051 nz_A = size(Am,2); 0052 nz_B = size(Bm,2); 0053 d_G = size(cg,1); 0054 N = size(Cov_ll_m,2); 0055 G = I * d_G; 0056 U = size(xa,1); 0057 0058 % redundancy 0059 R = G-U; 0060 if R < 0 0061 disp('not enough observations') 0062 return; 0063 end 0064 %% initialization --------------------------------------------------------- 0065 nu = 0; % index of iterations 0066 la = lm; % matrix of approximate fitted observations 0067 lnu = la; % matrix of current fitted observations 0068 xnu = xa; % vector of approxmate parameters 0069 s = 0; % iteration indicator 0070 0071 Am = spalloc(G,U,G*d_G*nz_A); 0072 Bmt = spalloc(G,N,G*d_G*nz_B); 0073 Cov_ll = spalloc(N,N,N*d_I^2); 0074 W_gg = spalloc(G,G,G*d_G^2); 0075 0076 for iter = 1: maxiter 0077 0078 for i=1:I 0079 % residual constraints, Jacobains 0080 [cg_i,Amt_i,Bmt_i] = cg_f(lm(i,:)',lnu(i,:)',xnu); 0081 Am((i-1)*d_G+1:i*d_G,:) = Amt_i; %#ok<*SPRIX> 0082 Bmt((i-1)*d_G+1:i*d_G,(i-1)*d_I+1:i*d_I)= Bmt_i; 0083 cg((i-1)*d_G+1:i*d_G) = cg_i; 0084 Cov_ll_i = reshape(Cov_ll_m(i,:),d_I,d_I); 0085 % weight matrix of lv 0086 Cov_ll((i-1)*d_I+1:i*d_I,(i-1)*d_I+1:i*d_I) ... 0087 = Cov_ll_i; 0088 % weights of constraints 0089 W_gg_i = inv(Bmt_i * Cov_ll_i * Bmt_i'); 0090 W_gg((i-1)*d_G+1:i*d_G,(i-1)*d_G+1:i*d_G) = W_gg_i; 0091 end 0092 Bm=Bmt'; 0093 ABWB = Am' * W_gg; 0094 Nm = ABWB * Am; % normal equation matrix 0095 nv = ABWB * cg; % right hand sides 0096 0097 % dNmis = diag(1./sqrt(diag(Nm))); % condition Nm 0098 % T_Nm = dNmis * Nm * dNmis; 0099 lambda_N = eigs(Nm); 0100 if abs(log(lambda_N(1)/lambda_N(U))) > 10 0101 disp('normal equation matrix nearly singular') 0102 return; 0103 end 0104 Cov_xx = inv(Nm); % CovM of parameters 0105 delta_x = Nm \ nv; % correction of parameters 0106 0107 nu = nu+1; 0108 0109 % check for final iteration 0110 if max(abs(delta_x(:)./sx(:))) < Tx || nu == maxiter 0111 s = 2; 0112 end 0113 % correction of parameters 0114 xnu = ux(xnu,delta_x); 0115 0116 % dx = delta_x' 0117 % nu_x = [nu,xnu'] 0118 0119 vv = lnu - lm; % v^a 0120 vv_vector = reshape(vv',I*d_I,1); 0121 % residual constraints, Jacobains 0122 delta_l = Cov_ll * Bm * W_gg ... 0123 * (cg - Am * delta_x) - vv_vector; 0124 % delta_ls = delta_l'; 0125 lnu = lnu + reshape(delta_l,d_I,I)'; % fitted observations 0126 % correction to fitted observations 0127 vv = lnu - lm; % residual matrix 0128 vv_vector = reshape(vv',I*d_I,1); 0129 % Omega 0130 % from residuals of constraints, 0131 % also works if covariance matrix of observations is singular 0132 cg = cg - Am * delta_x; 0133 Omega = cg' * W_gg * cg; 0134 0135 if s==2 0136 break; 0137 end 0138 end 0139 if R > 0 0140 sigma_0q = Omega/R; 0141 else 0142 sigma_0q = 1; 0143 end 0144 est_x = xnu; 0145 0146 0147 %% diagnostics (useful if observations are groupwise uncorrelated) -------- 0148 0149 [~,Xv,Rim,nabla_lv,muv,muv1,Uv1q,Uv2] = ... 0150 diagnostics_GHM_constraints_multi_d... 0151 (rU,I,d_I,d_G,Am,Bm,Cov_xx,Nm,W_gg,vv_vector);