MAP（最大后验）估计理论（2）以及相关应用

4、非线性高斯模型 MAP

1 问题设定

假设观测模型：

y = h ( x ) + v , v ∼ N ( 0 , R ) \mathbf{y} = h(\mathbf{x}) + \mathbf{v}, \quad \mathbf{v} \sim \mathcal{N}(0, R) y=h(x)+v,v∼N(0,R)

y ∈ R m \mathbf{y} \in \mathbb{R}^m y∈Rm：观测向量
x ∈ R n \mathbf{x} \in \mathbb{R}^n x∈Rn：未知状态向量
h : R n → R m h: \mathbb{R}^n \to \mathbb{R}^m h:Rn→Rm：非线性观测函数
R ∈ R m × m R \in \mathbb{R}^{m \times m} R∈Rm×m：观测噪声协方差

先验：

x ∼ N ( μ 0 , Σ 0 ) \mathbf{x} \sim \mathcal{N}(\mu_0, \Sigma_0) x∼N(μ0,Σ0)

MAP 目标函数：

x ^ MAP = arg ⁡ min ⁡ x f ( x ) = ( y − h ( x ) ) T R − 1 ( y − h ( x ) ) + ( x − μ 0 ) T Σ 0 − 1 ( x − μ 0 ) \hat{\mathbf{x}}\text{MAP} = \arg \min\mathbf{x} f(\mathbf{x}) = (\mathbf{y}-h(\mathbf{x}))^T R^{-1} (\mathbf{y}-h(\mathbf{x})) + (\mathbf{x}-\mu_0)^T \Sigma_0^{-1} (\mathbf{x}-\mu_0) x^MAP=argxminf(x)=(y−h(x))TR−1(y−h(x))+(x−μ0)TΣ0−1(x−μ0)

第一项 = 数据残差
第二项 = 先验正则
权重 = 协方差逆 → 信息矩阵

核心：非线性最小二乘问题

2 高斯-牛顿迭代推导

非线性函数 h ( x ) h(\mathbf{x}) h(x) 无闭式解，需要迭代方法。

2.1 线性化

在当前迭代点 x k \mathbf{x}_k xk 附近，对 h ( x ) h(\mathbf{x}) h(x) 做一阶泰勒展开：

h ( x ) ≈ h ( x k ) + J k ( x − x k ) h(\mathbf{x}) \approx h(\mathbf{x}_k) + J_k (\mathbf{x} - \mathbf{x}_k) h(x)≈h(xk)+Jk(x−xk)

J k = ∂ h ∂ x ∣ x k ∈ R m × n J_k = \frac{\partial h}{\partial \mathbf{x}} \big|_{\mathbf{x}_k} \in \mathbb{R}^{m \times n} Jk=∂x∂h xk∈Rm×n：雅可比矩阵

残差：

r ( x ) = y − h ( x ) ≈ y − h ( x k ) − J k ( x − x k ) r(\mathbf{x}) = \mathbf{y} - h(\mathbf{x}) \approx \mathbf{y} - h(\mathbf{x}_k) - J_k (\mathbf{x}-\mathbf{x}_k) r(x)=y−h(x)≈y−h(xk)−Jk(x−xk)

2.2 构造二次近似

MAP 代价函数：

f ( x ) = r ( x ) T R − 1 r ( x ) + ( x − μ 0 ) T Σ 0 − 1 ( x − μ 0 ) f(\mathbf{x}) = r(\mathbf{x})^T R^{-1} r(\mathbf{x}) + (\mathbf{x}-\mu_0)^T \Sigma_0^{-1} (\mathbf{x}-\mu_0) f(x)=r(x)TR−1r(x)+(x−μ0)TΣ0−1(x−μ0)

线性化后：
f ( x ) ≈ ( y − h ( x k ) ⏟ r k − J k δ ) T R − 1 ( y − h ( x k ) ⏟ r k − J k δ ) + ( x k + δ − μ 0 ) T Σ 0 − 1 ( x k + δ − μ 0 ) f(\mathbf{x}) \approx (\underbrace{\mathbf{y} - h(\mathbf{x}k)}{r_k} - J_k \delta)^T R^{-1} (\underbrace{\mathbf{y} - h(\mathbf{x}k)}{r_k} - J_k \delta) + (\mathbf{x}_k + \delta - \mu_0)^T \Sigma_0^{-1} (\mathbf{x}_k + \delta - \mu_0) f(x)≈(rk y−h(xk)−Jkδ)TR−1(rk y−h(xk)−Jkδ)+(xk+δ−μ0)TΣ0−1(xk+δ−μ0)

δ = x − x k \delta = \mathbf{x} - \mathbf{x}_k δ=x−xk

2.3 梯度设为零求解 δ \delta δ

线性化残差：

r ( x ) = y − h ( x ) ≈ r k − J k δ , r k = y − h ( x k ) , δ = x − x k r(\mathbf{x}) = \mathbf{y} - h(\mathbf{x}) \approx r_k - J_k \delta, \quad r_k = \mathbf{y} - h(\mathbf{x}_k), \ \delta = \mathbf{x} - \mathbf{x}_k r(x)=y−h(x)≈rk−Jkδ,rk=y−h(xk), δ=x−xk

代价函数（MAP）：

f ( x ) = r ( x ) T R − 1 r ( x ) + ( x − μ 0 ) T Σ 0 − 1 ( x − μ 0 ) f(\mathbf{x}) = r(\mathbf{x})^T R^{-1} r(\mathbf{x}) + (\mathbf{x} - \mu_0)^T \Sigma_0^{-1} (\mathbf{x} - \mu_0) f(x)=r(x)TR−1r(x)+(x−μ0)TΣ0−1(x−μ0)

线性化后：

f ( x k + δ ) ≈ ( r k − J k δ ) T R − 1 ( r k − J k δ ) + ( x k + δ − μ 0 ) T Σ 0 − 1 ( x k + δ − μ 0 ) f(\mathbf{x}_k + \delta) \approx (r_k - J_k \delta)^T R^{-1} (r_k - J_k \delta) + (\mathbf{x}_k + \delta - \mu_0)^T \Sigma_0^{-1} (\mathbf{x}_k + \delta - \mu_0) f(xk+δ)≈(rk−Jkδ)TR−1(rk−Jkδ)+(xk+δ−μ0)TΣ0−1(xk+δ−μ0)

展开第一项

考虑平方项：

( r k − J k δ ) T R − 1 ( r k − J k δ ) = r k T R − 1 r k − r k T R − 1 J k δ − δ T J k T R − 1 r k + δ T J k T R − 1 J k δ (r_k - J_k \delta)^T R^{-1} (r_k - J_k \delta) = r_k^T R^{-1} r_k - r_k^T R^{-1} J_k \delta - \delta^T J_k^T R^{-1} r_k + \delta^T J_k^T R^{-1} J_k \delta (rk−Jkδ)TR−1(rk−Jkδ)=rkTR−1rk−rkTR−1Jkδ−δTJkTR−1rk+δTJkTR−1Jkδ

注意：中间两项相等（标量）：

− r k T R − 1 J k δ − δ T J k T R − 1 r k = − 2 δ T J k T R − 1 r k -r_k^T R^{-1} J_k \delta - \delta^T J_k^T R^{-1} r_k = -2 \delta^T J_k^T R^{-1} r_k −rkTR−1Jkδ−δTJkTR−1rk=−2δTJkTR−1rk

所以第一项展开为：

( r k − J k δ ) T R − 1 ( r k − J k δ ) = r k T R − 1 r k − 2 δ T J k T R − 1 r k + δ T J k T R − 1 J k δ (r_k - J_k \delta)^T R^{-1} (r_k - J_k \delta) = r_k^T R^{-1} r_k - 2 \delta^T J_k^T R^{-1} r_k + \delta^T J_k^T R^{-1} J_k \delta (rk−Jkδ)TR−1(rk−Jkδ)=rkTR−1rk−2δTJkTR−1rk+δTJkTR−1Jkδ

展开第二项

先验项：

( x k + δ − μ 0 ) T Σ 0 − 1 ( x k + δ − μ 0 ) = ( δ + ( x k − μ 0 ) ) T Σ 0 − 1 ( δ + ( x k − μ 0 ) ) (\mathbf{x}_k + \delta - \mu_0)^T \Sigma_0^{-1} (\mathbf{x}_k + \delta - \mu_0) = (\delta + (\mathbf{x}_k - \mu_0))^T \Sigma_0^{-1} (\delta + (\mathbf{x}_k - \mu_0)) (xk+δ−μ0)TΣ0−1(xk+δ−μ0)=(δ+(xk−μ0))TΣ0−1(δ+(xk−μ0))

展开：

= ( x k − μ 0 ) T Σ 0 − 1 ( x k − μ 0 ) + 2 δ T Σ 0 − 1 ( x k − μ 0 ) + δ T Σ 0 − 1 δ = (\mathbf{x}_k - \mu_0)^T \Sigma_0^{-1} (\mathbf{x}_k - \mu_0) + 2 \delta^T \Sigma_0^{-1} (\mathbf{x}_k - \mu_0) + \delta^T \Sigma_0^{-1} \delta =(xk−μ0)TΣ0−1(xk−μ0)+2δTΣ0−1(xk−μ0)+δTΣ0−1δ

对 δ \delta δ 求梯度

代价函数关于 δ \delta δ 的梯度：

f ( δ ) = δ T J k T R − 1 J k δ − 2 δ T J k T R − 1 r k + δ T Σ 0 − 1 δ + 2 δ T Σ 0 − 1 ( x k − μ 0 ) + const f(\delta) = \delta^T J_k^T R^{-1} J_k \delta - 2 \delta^T J_k^T R^{-1} r_k + \delta^T \Sigma_0^{-1} \delta + 2 \delta^T \Sigma_0^{-1} (\mathbf{x}_k - \mu_0) + \text{const} f(δ)=δTJkTR−1Jkδ−2δTJkTR−1rk+δTΣ0−1δ+2δTΣ0−1(xk−μ0)+const

对 δ \delta δ 求梯度（矩阵微积分规则： ∇ δ ( δ T A δ ) = 2 A δ , ∇ δ ( b T δ ) = b \nabla_\delta (\delta^T A \delta) = 2 A \delta, \ \nabla_\delta (b^T \delta) = b ∇δ(δTAδ)=2Aδ, ∇δ(bTδ)=b）：

∇ δ ( δ T J k T R − 1 J k δ ) = 2 J k T R − 1 J k δ \nabla_\delta (\delta^T J_k^T R^{-1} J_k \delta) = 2 J_k^T R^{-1} J_k \delta ∇δ(δTJkTR−1Jkδ)=2JkTR−1Jkδ
∇ δ ( − 2 δ T J k T R − 1 r k ) = − 2 J k T R − 1 r k \nabla_\delta (-2 \delta^T J_k^T R^{-1} r_k) = -2 J_k^T R^{-1} r_k ∇δ(−2δTJkTR−1rk)=−2JkTR−1rk
∇ δ ( δ T Σ 0 − 1 δ ) = 2 Σ 0 − 1 δ \nabla_\delta (\delta^T \Sigma_0^{-1} \delta) = 2 \Sigma_0^{-1} \delta ∇δ(δTΣ0−1δ)=2Σ0−1δ
∇ δ ( 2 δ T Σ 0 − 1 ( x k − μ 0 ) ) = 2 Σ 0 − 1 ( x k − μ 0 ) \nabla_\delta (2 \delta^T \Sigma_0^{-1} (\mathbf{x}_k - \mu_0)) = 2 \Sigma_0^{-1} (\mathbf{x}_k - \mu_0) ∇δ(2δTΣ0−1(xk−μ0))=2Σ0−1(xk−μ0)

加起来：

∇ δ f ≈ 2 J k T R − 1 J k δ − 2 J k T R − 1 r k + 2 Σ 0 − 1 δ + 2 Σ 0 − 1 ( x k − μ 0 ) \nabla_\delta f \approx 2 J_k^T R^{-1} J_k \delta - 2 J_k^T R^{-1} r_k + 2 \Sigma_0^{-1} \delta + 2 \Sigma_0^{-1} (\mathbf{x}_k - \mu_0) ∇δf≈2JkTR−1Jkδ−2JkTR−1rk+2Σ0−1δ+2Σ0−1(xk−μ0)

合并：

∇ δ f ≈ − 2 J k T R − 1 ( r k − J k δ ) + 2 Σ 0 − 1 ( δ + x k − μ 0 ) \nabla_\delta f \approx -2 J_k^T R^{-1} (r_k - J_k \delta) + 2 \Sigma_0^{-1} (\delta + \mathbf{x}_k - \mu_0) ∇δf≈−2JkTR−1(rk−Jkδ)+2Σ0−1(δ+xk−μ0)

令梯度为零：

∇ δ f = 0 \nabla_\delta f = 0 ∇δf=0

整理：

( J k T R − 1 J k + Σ 0 − 1 ) δ = J k T R − 1 r k + Σ 0 − 1 ( μ 0 − x k ) (J_k^T R^{-1} J_k + \Sigma_0^{-1}) \delta = J_k^T R^{-1} r_k + \Sigma_0^{-1} (\mu_0 - \mathbf{x}_k) (JkTR−1Jk+Σ0−1)δ=JkTR−1rk+Σ0−1(μ0−xk)

注意：这是线性系统，可以通过 Cholesky 或 QR 求解

2.4 更新迭代

x k + 1 = x k + δ \mathbf{x}_{k+1} = \mathbf{x}_k + \delta xk+1=xk+δ

循环迭代直到 ∣ δ ∣ < ϵ |\delta| < \epsilon ∣δ∣<ϵ 或代价函数收敛
高斯-牛顿核心思想：将非线性最小二乘问题线性化，逐步逼近 MAP

3 Levenberg-Marquardt (LM) 改进

高斯-牛顿在残差较大或初值远离真实解时可能不收敛 → LM 增加阻尼项：

( J k T R − 1 J k + Σ 0 − 1 + λ I ) δ = J k T R − 1 r k + Σ 0 − 1 ( μ 0 − x k ) (J_k^T R^{-1} J_k + \Sigma_0^{-1} + \lambda I) \delta = J_k^T R^{-1} r_k + \Sigma_0^{-1} (\mu_0 - \mathbf{x}_k) (JkTR−1Jk+Σ0−1+λI)δ=JkTR−1rk+Σ0−1(μ0−xk)

λ > 0 \lambda > 0 λ>0：阻尼因子
λ → 0 \lambda \to 0 λ→0 → 高斯-牛顿
λ → ∞ \lambda \to \infty λ→∞ → 梯度下降
自适应调整 λ \lambda λ 可以保证收敛性

4 数值稳定性

不要直接求逆矩阵
使用 Cholesky 分解 求解对称正定系统：

A δ = b , A = J k T R − 1 J k + Σ 0 − 1 A \delta = b, \quad A = J_k^T R^{-1} J_k + \Sigma_0^{-1} Aδ=b,A=JkTR−1Jk+Σ0−1

Cholesky 分解 ( A = L L T A = L L^T A=LLT)
前向代解 ( L z = b L z = b Lz=b)
后向代解 ( L T δ = z L^T \delta = z LTδ=z)

优点：
- 避免显式求逆
- 提升数值稳定性
- 可处理稀疏大规模系统 → SLAM/LIO 后端

5 先验的几何作用

残差 = 数据约束 + 先验约束
先验项 ( Σ 0 − 1 \Sigma_0^{-1} Σ0−1) = 正则化 → 限制解空间
高斯噪声假设 → 残差平方 = 负对数后验
MAP = 残差平方最小化 = 最大后验概率点

在 SLAM 中：

观测残差 → 图中的边

先验 → 锚定节点（初始位姿或闭环约束）

LM / GN → 迭代优化整个图

6 总结迭代公式

计算残差：

r k = y − h ( x k ) r_k = y - h(x_k) rk=y−h(xk)

计算雅可比矩阵：

J k = ∂ h ∂ x ∣ x k J_k = \frac{\partial h}{\partial x} \Big|_{x_k} Jk=∂x∂h xk

构建线性系统：

A = J k T R − 1 J k + Σ 0 − 1 , b = J k T R − 1 r k + Σ 0 − 1 ( μ 0 − x k ) A = J_k^T R^{-1} J_k + \Sigma_0^{-1}, \quad b = J_k^T R^{-1} r_k + \Sigma_0^{-1} (\mu_0 - x_k) A=JkTR−1Jk+Σ0−1,b=JkTR−1rk+Σ0−1(μ0−xk)

求解 δ \delta δ：

A δ = b A \delta = b Aδ=b

更新：

x k + 1 = x k + δ x_{k+1} = x_k + \delta xk+1=xk+δ

若使用 LM：

A LM = A + λ I A_\text{LM} = A + \lambda I ALM=A+λI

7 直观理解

高斯残差 = 残差平方 → 负对数似然
先验 = 残差平方 → 正则化
雅可比矩阵 = 本次线性化灵敏度
高斯-牛顿 = 每次沿局部二次近似下降
LM = 防止远离初值发散

MAP = "最可能的状态 + 不确定性加权的平衡"

8 非线性高斯 MAP的C++ Eigen 模板示例

下面是一个 C++ Eigen 实现模板 ，支持 非线性高斯 MAP，包含：

高斯-牛顿迭代求解
可选 LM 阻尼
支持先验均值和协方差
使用 Cholesky 求解，数值稳定
输出 MAP 均值和条件协方差

下面是一个完整示例：

cpp 复制代码

#include <Eigen/Dense>
#include <iostream>
#include <functional>

// 非线性高斯 MAP 求解器
struct NonlinearMAPSolver {
    using Vec = Eigen::VectorXd;
    using Mat = Eigen::MatrixXd;

    // h(x) = 观测函数，返回残差 r = y - h(x)
    std::function<Vec(const Vec& x)> residualFunc;
    // 雅可比矩阵 J = dr/dx
    std::function<Mat(const Vec& x)> jacobianFunc;

    Vec x0;          // 初值
    Vec mu0;         // 先验均值
    Mat Sigma0;      // 先验协方差
    Mat R;           // 观测协方差
    int maxIter = 50;
    double tol = 1e-6;
    bool useLM = false;
    double lambda = 1e-3;

    // 求解 MAP
    Vec solve(Mat& SigmaMAP) {
        Vec x = x0;
        Mat Sigma0Inv = Sigma0.inverse();
        Mat Rinv = R.inverse();

        for (int iter = 0; iter < maxIter; ++iter) {
            Vec r = residualFunc(x);           // 残差
            Mat J = jacobianFunc(x);           // 雅可比

            Mat A = J.transpose() * Rinv * J + Sigma0Inv;
            Vec b = J.transpose() * Rinv * r + Sigma0Inv * (mu0 - x);

            if (useLM) {
                A += lambda * Mat::Identity(x.size(), x.size());
            }

            // Cholesky 求解线性系统 A*delta = b
            Eigen::LLT<Mat> llt(A);
            if(llt.info() != Eigen::Success){
                std::cerr << "Cholesky decomposition failed!" << std::endl;
                break;
            }
            Vec delta = llt.solve(b);

            x += delta;

            if (delta.norm() < tol) break;
        }

        // 计算后验协方差
        Mat J_final = jacobianFunc(x);
        Mat A_final = J_final.transpose() * R.inverse() * J_final + Sigma0.inverse();
        Eigen::LLT<Mat> llt_final(A_final);
        SigmaMAP = llt_final.solve(Mat::Identity(x.size(), x.size()));

        return x;
    }
};

// 示例使用
int main() {
    using Vec = Eigen::VectorXd;
    using Mat = Eigen::MatrixXd;

    int n = 2; // 状态维度
    int m = 2; // 观测维度

    Vec mu0(n);
    mu0 << 0, 0;
    Mat Sigma0 = Mat::Identity(n,n) * 0.1; // 先验协方差

    Mat R = Mat::Identity(m,m) * 0.01;     // 观测协方差
    Vec y(m);
    y << 1.0, 2.0;                         // 观测值

    // 定义非线性观测函数 h(x)
    auto h = [y](const Vec& x) -> Vec {
        Vec hx(2);
        hx(0) = std::sin(x(0)) + x(1);
        hx(1) = x(0) * x(1);
        return y - hx; // 返回 r = y - h(x)
    };

    // 雅可比矩阵
    auto J = [](const Vec& x) -> Mat {
        Mat J(2,2);
        J(0,0) = -std::cos(x(0));
        J(0,1) = -1.0;
        J(1,0) = -x(1);
        J(1,1) = -x(0);
        return J;
    };

    NonlinearMAPSolver solver;
    solver.x0 = mu0;
    solver.mu0 = mu0;
    solver.Sigma0 = Sigma0;
    solver.R = R;
    solver.residualFunc = h;
    solver.jacobianFunc = J;
    solver.useLM = true;

    Mat SigmaMAP;
    Vec xMAP = solver.solve(SigmaMAP);

    std::cout << "MAP estimate x:\n" << xMAP << "\n";
    std::cout << "Posterior covariance Sigma:\n" << SigmaMAP << "\n";

    return 0;
}

特性说明

高斯-牛顿/LM：
- useLM = false → 高斯-牛顿
- useLM = true → Levenberg-Marquardt
先验处理：
- 使用 Sigma0 和 mu0 构造正则项
- 与观测残差加权融合
数值稳定：
- 使用 Cholesky 求解线性系统
- 避免显式求逆
输出：
- xMAP：MAP 均值
- SigmaMAP：条件协方差（后验不确定性）
可扩展：
- 支持任意维度
- 可直接用于 SLAM / LIO 后端优化

9 线性高斯版 C++ Eigen 模板示例

下面是一个 完整 C++ Eigen 模板，同时支持：

线性高斯 MAP（闭式解）
非线性高斯 MAP（高斯-牛顿 / LM 迭代解）
输出 MAP 均值 和 后验协方差
使用 Cholesky 保证数值稳定

下面是完整示例：