kappa=3; %已在您的代码中定义

% 定义窗口大小
windowSize = (2*kappa+1);

% 初始化梯度估计值
[rows, cols] = size(wrappedPhase);
phi_y = zeros(rows, cols);
phi_x = zeros(rows, cols);

% 遍历每个窗口
for m = 1+kappa:rows-kappa
    for n = 1+kappa:cols-kappa
        % 提取局部窗口
        Z_mn = wrappedPhase(m-kappa:m+kappa, n-kappa:n+kappa);

        % SVD分解
        [U, Sigma, V] = svd(Z_mn);
        % 降噪处理 - 仅保留最大的奇异值
        Sigma_denoised = zeros(size(Sigma));
        Sigma_denoised(1, 1) = Sigma(1, 1);
        % 构造降噪后的矩阵
        Z_denoised = U * Sigma_denoised * V';
        % 提取子矩阵
        Y_0 = Z_denoised(1:2*kappa, 1:2*kappa);
        Y_1 = Z_denoised(2:2*kappa+1, 1:2*kappa);
        Y_2 = Z_denoised(1:2*kappa, 2:2*kappa+1);
        % 对Y_0进行奇异值分解
        [U_00, ~, V_00] = svd(Y_0);
        % 根据分解结果构造Y_0*，Y_1*，Y_2*
        Y_0_star = U_00' * Y_0 * V_00;
        Y_1_star = U_00' * Y_1 * V_00;
        Y_2_star = U_00' * Y_2 * V_00;
        % 计算Y1*和Y2*的伪逆
        y_1_plus = pinv(Y_1_star);
        y_2_plus = pinv(Y_2_star);
        % 计算相位梯度估计值
        % 计算相位梯度估计值
        angle_matrix_y = angle(conj(y_1_plus * Y_0_star));
        angle_matrix_x = angle(conj(y_2_plus * Y_0_star));
        phi_y(m, n) = mean(diag(angle_matrix_y));
        phi_x(m, n) = mean(diag(angle_matrix_x));
    end
end

效果不是很好，放弃了， 

参考文献：

相位解缠的 CKF局部多项式系数递推估计法 谢 先 明 ,孙 玉 铮 ,梁 小 星 ,曾 庆 宁 ,郑 展 恒