相关内容： 1、MAP（最大后验）估计理论（1）以及相关应用
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
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+δ
A
LM
=
A
+
λ
I
A_\\text{LM} = A + \\lambda I
ALM=A+λI
7 直观理解
MAP = “最可能的状态 + 不确定性加权的平衡”
8 非线性高斯 MAP的C++ Eigen 模板示例
下面是一个 C++ Eigen 实现模板，支持 非线性高斯 MAP，包含：
下面是一个完整示例：
#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 模板，同时支持：
下面是完整示例：
#include <Eigen/Dense>
#include <iostream>
#include <functional>
struct MAPSolver {
using Vec = Eigen::VectorXd;
using Mat = Eigen::MatrixXd;
// ——————-
// 线性高斯 MAP
// ——————-
// y = H x + v, v ~ N(0, R)
// x ~ N(mu0, Sigma0)
static void solveLinearMAP(const Mat& H, const Vec& y,
const Mat& R, const Vec& mu0, const Mat& Sigma0,
Vec& xMAP, Mat& SigmaMAP)
{
Mat Sigma0Inv = Sigma0.inverse();
Mat Rinv = R.inverse();
Mat A = Sigma0Inv + H.transpose() * Rinv * H;
Vec b = Sigma0Inv * mu0 + H.transpose() * Rinv * y;
Eigen::LLT<Mat> llt(A);
if (llt.info() != Eigen::Success) {
throw std::runtime_error("Cholesky decomposition failed in linear MAP!");
}
xMAP = llt.solve(b);
SigmaMAP = llt.solve(Mat::Identity(A.rows(), A.cols())); // 后验协方差
}
// ——————-
// 非线性高斯 MAP
// ——————-
struct NonlinearOptions {
int maxIter = 50;
double tol = 1e-6;
bool useLM = false;
double lambda = 1e-3;
};
static Vec solveNonlinearMAP(
const std::function<Vec(const Vec&)>& residualFunc,
const std::function<Mat(const Vec&)>& jacobianFunc,
const Vec& x0,
const Vec& mu0,
const Mat& Sigma0,
const Mat& R,
Mat& SigmaMAP,
const NonlinearOptions& opt = NonlinearOptions()
) {
Vec x = x0;
Mat Sigma0Inv = Sigma0.inverse();
Mat Rinv = R.inverse();
for (int iter = 0; iter < opt.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 (opt.useLM) {
A += opt.lambda * Mat::Identity(x.size(), x.size());
}
Eigen::LLT<Mat> llt(A);
if (llt.info() != Eigen::Success) {
throw std::runtime_error("Cholesky decomposition failed in nonlinear MAP!");
}
Vec delta = llt.solve(b);
x += delta;
if (delta.norm() < opt.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(A_final.rows(), A_final.cols()));
return x;
}
};
// ——————-
// 示例
// ——————-
int main() {
using Vec = Eigen::VectorXd;
using Mat = Eigen::MatrixXd;
// 线性示例
int n = 2, m = 2;
Vec y(m); y << 1.0, 2.0;
Mat H(m,n); H << 1, 0, 0, 1;
Mat R = Mat::Identity(m,m) * 0.01;
Vec mu0(n); mu0 << 0, 0;
Mat Sigma0 = Mat::Identity(n,n) * 0.1;
Vec xMAP;
Mat SigmaMAP;
MAPSolver::solveLinearMAP(H, y, R, mu0, Sigma0, xMAP, SigmaMAP);
std::cout << "Linear MAP estimate:\\n" << xMAP << "\\n";
std::cout << "Linear MAP covariance:\\n" << SigmaMAP << "\\n\\n";
// 非线性示例
auto h = [](const Vec& x) -> Vec {
Vec hx(2);
hx(0) = std::sin(x(0)) + x(1);
hx(1) = x(0) * x(1);
return Vec::Constant(2, 0.0) – hx; // residual = y – h(x), y assumed absorbed in caller
};
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;
};
MAPSolver::NonlinearOptions opt;
opt.useLM = true;
opt.lambda = 1e-3;
Vec x0 = mu0;
Vec xMAP_nonlin = MAPSolver::solveNonlinearMAP(h, J, x0, mu0, Sigma0, R, SigmaMAP, opt);
std::cout << "Nonlinear MAP estimate:\\n" << xMAP_nonlin << "\\n";
std::cout << "Nonlinear MAP covariance:\\n" << SigmaMAP << "\\n";
return 0;
}
** 模板特点**
统一接口：线性 / 非线性 MAP 均可使用
高斯-牛顿 / LM：非线性迭代求解
先验均值与协方差直接纳入计算
Cholesky 求解：数值稳定
输出条件协方差：可用于后验不确定性分析
可扩展：
- 可处理任意维度状态
- 可用于 SLAM / LIO 后端优化
- 可替换 residualFunc 和 jacobianFunc 适配实际观测
