高斯牛顿法

1. 问题定义

求解非线性最小二乘问题：

其中：

x∈Rnmathbf{x} in mathbb{R}^nx∈Rn：待优化参数ri(x)r_i(mathbf{x})ri​(x)：第i个残差函数r(x)=[r1(x),…,rm(x)]⊤mathbf{r}(mathbf{x}) = [r_1(mathbf{x}), dots, r_m(mathbf{x})]^ opr(x)=[r1​(x),…,rm​(x)]⊤：残差向量

2. 核心思想：局部线性化

在当前点xkmathbf{x}_kxk​处对残差函数进行一阶泰勒展开：

其中雅可比矩阵：

3. 目标函数近似

将线性化后的残差代入目标函数：

4. 求解最优增量

对近似目标函数求导并令导数为零：

得到高斯牛顿方程：

5. 算法流程

输入：初始值x0mathbf{x}_0x0​，最大迭代次数KKK，容忍度ϵepsilonϵ

For k=0,1,…,K−1k = 0, 1, dots, K-1k=0,1,…,K−1：

计算残差：rk=r(xk)mathbf{r}_k = mathbf{r}(mathbf{x}_k)rk​=r(xk​)计算雅可比：Jk=J(xk)mathbf{J}_k = mathbf{J}(mathbf{x}_k)Jk​=J(xk​)构造线性系统：Hk=Jk⊤Jkmathbf{H}_k = mathbf{J}_k^ opmathbf{J}_kHk​=Jk⊤​Jk​，gk=−Jk⊤rkmathbf{g}_k = -mathbf{J}_k^ opmathbf{r}_kgk​=−Jk⊤​rk​求解增量：HkΔxk=gkmathbf{H}_kDeltamathbf{x}_k = mathbf{g}_kHk​Δxk​=gk​更新参数：xk+1=xk+Δxkmathbf{x}_{k+1} = mathbf{x}_k + Deltamathbf{x}_kxk+1​=xk​+Δxk​如果∥Δxk∥<ϵ|Deltamathbf{x}_k| < epsilon∥Δxk​∥<ϵ，退出循环

输出：最优解x∗mathbf{x}^*x∗

6. 对于一个简化的SLAM中的位姿估算问题，计算方法如下

rng(88);
n = 100;
% 生成归一化数据
source_x = linspace(0, 5, n)';
source_y = linspace(0, 5, n)';  % 统一尺度
source_pt = [source_x, source_y];

% 归一化
source_mean = mean(source_pt);
source_std = std(source_pt);
source_pt_norm = (source_pt - source_mean) ./ source_std;

% 真实参数（在归一化空间）
true_x = 0.2 / source_std(1);
true_y = 0.3 / source_std(2);  
true_theta = 10 * pi / 180;  % 10度

% 正确生成target数据
target_pt_norm = zeros(n, 2);
for i = 1:n
    % 标准刚体变换
    target_pt_norm(i, 1) = cos(true_theta) * source_pt_norm(i, 1) - sin(true_theta) * source_pt_norm(i, 2) + true_x;
    target_pt_norm(i, 2) = sin(true_theta) * source_pt_norm(i, 1) + cos(true_theta) * source_pt_norm(i, 2) + true_y;
end
target_pt_norm = target_pt_norm + 0.01 * randn(n, 2);

%% 正确的高斯牛顿法
function res = GN_correct(init, source, target, iterate, tol)
    res = init(:);
    [row, ~] = size(source);
    
    for i = 1:iterate
        H = zeros(3, 3);
        b = zeros(3, 1);
        total_cost = 0;
        
        for j = 1:row
            theta = res(3);
            
            % 变换后的点
            source_p = [cos(theta) * source(j, 1) - sin(theta) * source(j, 2) + res(1), 
                       sin(theta) * source(j, 1) + cos(theta) * source(j, 2) + res(2)];
            
            % 误差
            e = target(j, :) - source_p;
            
            % 正确的雅可比矩阵
            J = [1, 0, -sin(theta) * source(j, 1) - cos(theta) * source(j, 2);
                 0, 1,  cos(theta) * source(j, 1) - sin(theta) * source(j, 2)];
            
            H = H + J' * J;
            b = b + J' * e';
            total_cost = total_cost + e * e';
        end
        
        cost = total_cost / row;
        fprintf('迭代 %d: 代价=%.6f, ', i, cost);
        
        % 求解
        H_reg = H + eye(3) * 1e-8;
        dx = H_reg  b;
        
        res_old = res;
        res = res + dx;
        
        fprintf('res=[%.6f, %.6f, %.2f°], norm(dx)=%.6f
', ...
                res(1), res(2), res(3)*180/pi, norm(dx));
        
        if norm(dx) < tol
            fprintf('收敛!
');
            break;
        end
    end
end

%% 运行
fprintf('真实参数: x=%.3f, y=%.3f, theta=%.1f°
', true_x, true_y, true_theta*180/pi);
x_init = [0.0, 0.0, 5*pi/180];  % 从5度开始
x_result = GN_correct(x_init, source_pt_norm, target_pt_norm, 20, 1e-6);

% 转换回原始空间
x_original = [x_result(1) * source_std(1) + source_mean(1), 
              x_result(2) * source_std(2) + source_mean(2), 
              x_result(3)];
fprintf('
最终结果(原始空间): x=%.6f, y=%.6f, theta=%.2f°
', ...
        x_original(1), x_original(2), x_original(3)*180/pi);

最终运行结果(原始空间): x=2.699727, y=4.708631, theta=21.97°