状态估计问题
状态估计问题是SLAM、目标跟踪、机器人导航等领域的核心问题，其目标是通过测量数据估计系统的状态（例如位姿、速度等）。它通常通过优化方法进行求解。
批量状态估计与最大后验估计
- 批量状态估计
批量状态估计是通过所有观测数据一次性优化所有状态的过程：
x ∗ = arg max x p ( x ∣ z ) \mathbf{x}^*=\arg\max_\mathbf{x}p(\mathbf{x}|\mathbf{z}) x∗=argxmaxp(x∣z)
其中：- x 是状态变量（例如位姿、地图点）。
- z 是观测数据。
- 最大后验估计（MAP）
根据贝叶斯公式，后验概率为： p ( x ∣ z ) ∝ p ( z ∣ x ) p ( x ) p(\mathbf{x}|\mathbf{z})\propto p(\mathbf{z}|\mathbf{x})p(\mathbf{x}) p(x∣z)∝p(z∣x)p(x)
最大后验估计通过同时考虑观测数据（似然函数）和先验信息，得到更鲁棒的估计： x ∗ = arg max x [ log p ( z ∣ x ) + log p ( x ) ] \mathbf{x}^*=\arg\max_{\mathbf{x}}\left[\log p(\mathbf{z}|\mathbf{x})+\log p(\mathbf{x})\right] x∗=argxmax[logp(z∣x)+logp(x)]
最小二乘的引出
- 如果假设测量噪声是高斯分布，观测模型为：
z
i
=
h
i
(
x
)
+
n
i
,
n
i
∼
N
(
0
,
Σ
i
)
\mathbf{z}_i=h_i(\mathbf{x})+\mathbf{n}_i,\quad\mathbf{n}_i\sim\mathcal{N}(\mathbf{0},\mathbf{\Sigma}_i)
zi=hi(x)+ni,ni∼N(0,Σi)
则观测的似然函数为： p ( z ∣ x ) = ∏ i 1 2 π ∣ Σ i ∣ exp ( − 1 2 ∥ z i − h i ( x ) ∥ Σ i − 1 2 ) p(\mathbf{z}|\mathbf{x})=\prod_i\frac{1}{\sqrt{2\pi|\boldsymbol{\Sigma}_i|}}\exp\left(-\frac{1}{2}\|\mathbf{z}_i-h_i(\mathbf{x})\|_{\boldsymbol{\Sigma}_i^{-1}}^2\right) p(z∣x)=i∏2π∣Σi∣ 1exp(−21∥zi−hi(x)∥Σi−12) - 对数化后最大化后验概率等价于最小化加权的平方误差：
x
∗
=
arg
min
x
∑
i
∥
z
i
−
h
i
(
x
)
∥
Σ
i
−
1
2
\mathbf{x}^{*}=\arg\min_{\mathbf{x}}\sum_{i}\|\mathbf{z}_{i}-h_{i}(\mathbf{x})\|_{\Sigma_{i}^{-1}}^{2}
x∗=argxmini∑∥zi−hi(x)∥Σi−12
这就是最小二乘问题。
非线性最小二乘
非线性最小二乘问题出现在状态变量与观测模型之间是非线性关系时，其形式为：
x
∗
=
arg
min
x
1
2
∑
i
∥
z
i
−
h
i
(
x
)
∥
2
\mathbf{x}^*=\arg\min_\mathbf{x}\frac{1}{2}\sum_i\|\mathbf{z}_i-h_i(\mathbf{x})\|^2
x∗=argxmin21i∑∥zi−hi(x)∥2
由于目标函数