```
% Estimation part %
%-----------------%
rho_i = 2.5;
w_i = (d_i.^(-1)./sum(d_i.^(-1)))';
Max_iter = 1e2;
x_est = 2 * Border * rand(3,M) - Border; % 3xM
delta_i_est = 2 * Border * rand(N,1) - Border;
% 调整变量维度为三维
u_i = 2 * Border * rand(3,N) - Border;
u_i = u_i ./ vecnorm(u_i); % 单位向量
v_i = 2 * Border * rand(3,N) - Border;
h_i = 2 * Border * rand(3,N) - Border;
q_i = 2 * Border * rand(N,1) - Border;
lambda_i = 2 * Border * rand(3,N) - Border;
nu_i = 2 * Border * rand(3,N) - Border;
mu_i = 2 * Border * rand(N,1) - Border;
x_est(:,2) = 1e6 * ones(3,M); % 初始化
iter = 1;
while iter <= Max_iter && norm(x_est(:,end) - x_est(:,end-1)) > 1e-3
% 三维索引调整
v_prev = v_i(:,:,iter);
h_prev = h_i(:,:,iter);
% 位置估计(三维)
x_est(:,iter+1) = sum(w_i .* (a_i + v_prev - h_prev), 2);
for tt = 1:N
% 三维变量更新
delta_prev = delta_i_est(tt,iter);
u_prev = u_i(:,tt,iter);
v_prev_tt = v_i(:,tt,iter);
h_prev_tt = h_i(:,tt,iter);
q_prev = q_i(tt,iter);
lambda_prev = lambda_i(:,tt,iter);
nu_prev = nu_i(:,tt,iter);
mu_prev = mu_i(tt,iter);
% ADMM更新步骤(调整三维运算)
delta_i_est(tt,iter+1) = (rho_i * d_i(tt) * q_prev + ...
rho_i * u_prev' * h_prev_tt - d_i(tt) * mu_prev - ...
u_prev' * nu_prev)/(1 + rho_i + rho_i * d_i(tt)^2);
u_next = (rho_i * delta_i_est(tt,iter+1) * h_prev_tt + ...
rho_i * d_i(tt) * v_prev_tt - ...
delta_i_est(tt,iter+1) * nu_prev - ...
d_i(tt) * lambda_prev) / ...
(rho_i * (d_i(tt)^2 + delta_i_est(tt,iter+1)^2));
u_i(:,tt,iter+1) = u_next / max(1, norm(u_next));
v_i(:,tt,iter+1) = (x_est(:,iter+1) - a_i(:,tt) + lambda_prev + ...
rho_i * d_i(tt) * u_i(:,tt,iter+1)) / rho_i;
h_i(:,tt,iter+1) = (-x_est(:,iter+1) + a_i(:,tt) + nu_prev + ...
rho_i * delta_i_est(tt,iter+1) * u_i(:,tt,iter+1)) / rho_i;
q_i(tt,iter+1) = (rho_i * d_i(tt) * delta_i_est(tt,iter+1) + mu_prev + 1) / rho_i;
lambda_i(:,tt,iter+1) = lambda_prev + rho_i * (d_i(tt) * u_i(:,tt,iter+1) - v_i(:,tt,iter+1));
nu_i(:,tt,iter+1) = nu_prev + rho_i * (delta_i_est(tt,iter+1) * u_i(:,tt,iter+1) - h_i(:,tt,iter+1));
mu_i(tt,iter+1) = mu_prev + rho_i * (d_i(tt) * delta_i_est(tt,iter+1) - q_i(tt,iter+1));
end
iter = iter + 1;
end```解释每一行代码
最新发布