深入解析SLAM中的状态估计问题：从理论到C++实现
深入解析SLAM中的状态估计问题：从理论到C++实现
一、SLAM状态估计问题基础
1．SLAM问题建模与数学描述
在SLAM（Simultaneous Localization and Mapping，同时定位与地图构建）领域，精准的问题建模是解决状态估计问题的基石。SLAM问题主要由运动方程和观测方程构成。运动方程描述了机器人位姿随时间的变化，可表示为 x k = f ( x k − 1 , u k , w k ) x_k = f(x_{k - 1}, u_k, w_k) xk=f(xk−1,uk,wk)，其中 x k x_k xk 代表 k k k 时刻的机器人位姿， u k u_k uk 是输入的控制量， w k w_k wk 为运动噪声。观测方程则刻画了机器人在某一位姿下对环境中路标（landmark）的观测，即 z k , j = h ( x k , y j , v k , j ) z_{k,j} = h(x_k, y_j, v_{k,j}) zk,j=h(xk,yj,vk,j)，这里 z k , j z_{k,j} zk,j 是在 k k k 时刻对第 j j j 个路标的观测值， y j y_j yj 是路标的真实位置， v k , j v_{k,j} vk,j 是观测噪声。
状态变量 x k x_k xk 和观测变量 z k , j z_{k,j} zk,j 通常具有概率分布特性。在经典SLAM模型中，一般假设噪声项 w k w_k wk 和 v k , j v_{k,j} vk,j 满足零均值的高斯分布，即 w k ∼ N ( 0 , R k ) w_k \sim N(0, R_k) wk∼N(0,Rk)， v k , j ∼ N ( 0 , Q k , j ) v_{k,j} \sim N(0, Q_{k,j}) vk,j∼N(0,Qk,j)，其中 R k R_k Rk 和 Q k , j Q_{k,j} Qk,j 分别为协方差矩阵。多维高斯分布的数学表达为 p ( x ) = 1 ( 2 π ) n / 2 ∣ Σ ∣ 1 / 2 exp ( − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) ) p(x) = \frac{1}{(2\pi)^{n/2}|\Sigma|^{1/2}} \exp\left(-\frac{1}{2}(x - \mu)^T\Sigma^{-1}(x - \mu)\right) p(x)=(2π)n/2∣Σ∣