ch11_continuous_time

第十一章　连续时间轨迹估计 §

Chapter 11   Continuous-Time Trajectory Estimation §

---

11.1 动机与概述 / Motivation and Overview §

中文

前两部分的所有估计方法均在离散时间框架内进行：在固定时刻 $t_0,t_1,\dots ,t_K$ 估计位姿序列。这对很多应用已经足够，但有一个根本局限：

问题：如果需要查询任意时刻 $\tau$（不一定是测量时刻）的位姿怎么办？

例如，激光雷达扫描是在高频率下逐点采集的（“运动畸变”问题），若能在任意时刻插值位姿，可以对每个点单独去畸变。

连续时间轨迹估计的思路：

1. 用**随机微分方程（SDE）**定义轨迹的先验分布——鼓励轨迹平滑。
2. 用高斯过程（GP）表示该先验，使得在任意时刻均可以封闭形式求取后验均值和协方差。
3. 在离散测量时刻求解 MAP 估计，再用 GP 插值/外推任意查询时刻的状态。

本章的核心算法是 STEAM（Simultaneous Trajectory Estimation and Mapping）。

---

English

All estimation methods so far operate in discrete time: poses are estimated at fixed timestamps. The fundamental limitation: what if we need the pose at an arbitrary query time $\tau$ not coinciding with any measurement?

Continuous-time trajectory estimation addresses this by:

1. Defining a smooth prior via a stochastic differential equation (SDE).
2. Representing the prior as a Gaussian process (GP) that can be evaluated at any time.
3. Solving a discrete MAP problem at measurement times, then GP-interpolating/extrapolating at query times.

The resulting algorithm is called STEAM (Simultaneous Trajectory Estimation and Mapping).

---

11.1 运动先验 / Motion Prior §

11.1.1 随机微分方程 / Stochastic Differential Equation §

中文

理想情况下，我们希望用以下非线性 SDE 刻画轨迹先验：

$$\dot{\mathbf{T}}(t)=\mathbf{\varpi }(t{)}^{\wedge }\mathbf{T}(t),\text{\hspace{1em}(11.1a)}$$ $$\dot{\mathbf{\varpi }}(t)={\mathbf{w}}^{\prime }(t),{\mathbf{w}}^{\prime }(t)\sim \mathcal{G}\mathcal{P}(0,{\mathbf{Q}}_C^{\prime }\delta (t-t^{\prime })).\text{\hspace{1em}(11.1b)}$$

第一个方程是 $SE(3)$ 上的运动学方程；第二个方程表示广义角加速度由白噪声激励（在无噪声时广义速度为常数）。这称为**“加速度白噪声”（white-noise-on-acceleration）或常速先验**。

难点：运动学方程非线性，无法直接用第 3 章和第 4 章的线性 GP 工具。

解决思路：引入局部变量 ${\mathbf{\xi }}_k(t)\in \mathfrak{se}(3)$，在每个支撑时刻附近将全局非线性运动转换为局部线性问题，再用线性 GP 工具处理。

---

English

The ideal prior SDE is:

$$\dot{\mathbf{T}}(t)=\mathbf{\varpi }(t{)}^{\wedge }\mathbf{T}(t),\dot{\mathbf{\varpi }}(t)={\mathbf{w}}^{\prime }(t),{\mathbf{w}}^{\prime }(t)\sim \mathcal{G}\mathcal{P}(0,{\mathbf{Q}}_C^{\prime }\delta (t-t^{\prime })).$$

This “white-noise-on-acceleration” model is nonlinear due to the $SE(3)$ kinematics. The solution: introduce local variables that convert the problem to a linear one locally.

---

11.1.2 局部随机微分方程 / Local SDEs §

中文

在每个支撑时刻 $t_k$，引入局部位姿变量 ${\mathbf{\xi }}_k(t)\in \mathfrak{se}(3)$，定义为

$$\mathbf{T}(t)=\exp ({\mathbf{\xi }}_k(t{)}^{\wedge })\mathbf{T}(t_k),t\in [t_k,t_{k+1}).\text{\hspace{1em}(11.2)}$$

局部变量 ${\mathbf{\xi }}_k(t)$ 表示相对于 $\mathbf{T}(t_k)$ 的增量位姿，只要增量不太大，李代数近似就非常精确。

在局部变量上定义线性 SDE：

$${\ddot{\mathbf{\xi }}}_k(t)={\mathbf{w}}_k(t),{\mathbf{w}}_k(t)\sim \mathcal{G}\mathcal{P}(0,Q\delta (t-t^{\prime })).\text{\hspace{1em}(11.3)}$$

整理为一阶形式，设 ${\mathbf{\psi }}_k(t)={\dot{\mathbf{\xi }}}_k(t)$，Markov 状态 ${\mathbf{\gamma }}_k(t)=[{\mathbf{\xi }}_k(t{)}^T,{\mathbf{\psi }}_k(t{)}^T{]}^T$：

$$\frac{d}{dt}{\mathbf{\gamma }}_k(t)=\underset{\mathbf{A}}{\underbrace{\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]}}{\mathbf{\gamma }}_k(t)+\left[\begin{array}{c}0\\ 1\end{array}\right]{\mathbf{w}}_k(t).\text{\hspace{1em}(11.4)}$$

由于这是线性时不变 SDE，可以用第 3.4 节的结果封闭形式积分：

$${\mathbf{\gamma }}_k(t)\sim \mathcal{G}\mathcal{P}\left(\Phi (t,t_k){\check{\mathbf{\gamma }}}_k(t_k),\Phi (t,t_k)\check{\mathbf{P}}(t_k)\Phi (t,t_k{)}^T+\mathbf{Q}(t-t_k)\right),\text{\hspace{1em}(11.5)}$$

其中状态转移矩阵和累积协方差分别为

$$\Phi (t,t^{\prime })=\left[\begin{array}{cc}1& (t-t^{\prime })1\\ 0& 1\end{array}\right],t\ge t^{\prime },\text{\hspace{1em}(11.6)}$$ $$\mathbf{Q}(t-t^{\prime })=\left[\begin{array}{cc}\frac{1}{3}(t-t^{\prime }{)}^{3}Q& \frac{1}{2}(t-t^{\prime }{)}^{2}Q\\ \frac{1}{2}(t-t^{\prime }{)}^{2}Q& (t-t^{\prime })Q\end{array}\right],t\ge t^{\prime }.\text{\hspace{1em}(11.7)}$$

直觉：$\Phi$ 是匀速运动的传播矩阵：位姿 = 初始位姿 + 速度 × 时间；$\mathbf{Q}(\Delta t)$ 是随时间增长的不确定性（上左角是位置不确定性 $\propto \Delta t^3$，右下角是速度不确定性 $\propto \Delta t$）。

---

English

Define local pose variable ${\mathbf{\xi }}_k(t)=\ln (\mathbf{T}(t)\mathbf{T}(t_k{)}^{-1}{)}^{\vee }\in \mathfrak{se}(3)$ between times $t_k$ and $t_{k+1}$.

On this local variable, impose the linear SDE ${\ddot{\mathbf{\xi }}}_k(t)={\mathbf{w}}_k(t)$ (white noise on acceleration). In first-order Markovian form with state ${\mathbf{\gamma }}_k=[{\mathbf{\xi }}_k^T,{\dot{\mathbf{\xi }}}_k^T{]}^T$, this is a linear time-invariant system that can be stochastically integrated in closed form:

$${\mathbf{\gamma }}_k(t)\sim \mathcal{G}\mathcal{P}\left(\Phi (t,t_k){\check{\mathbf{\gamma }}}_k(t_k),\Phi (t,t_k)\check{\mathbf{P}}(t_k)\Phi (t,t_k{)}^T+\mathbf{Q}(t-t_k)\right).$$

The transition matrix $\Phi$ is the constant-velocity propagator; $\mathbf{Q}(\Delta t)$ grows as $\Delta t^3$ in position and $\Delta t$ in velocity.

---

11.1.3 误差项与代价函数 / Error Terms and Cost Function §

中文

将局部变量换回全局变量 $\{\mathbf{T}(t_k),\mathbf{\varpi }(t_k)\}$：

$${\mathbf{\xi }}_k(t)=\ln (\mathbf{T}(t)\mathbf{T}(t_k{)}^{-1}{)}^{\vee },{\mathbf{\psi }}_k(t)=\mathcal{J}({\mathbf{\xi }}_k(t){)}^{-1}\mathbf{\varpi }(t),\text{\hspace{1em}(11.8)}$$

其中 $\mathcal{J}(\cdot )$ 是 $SE(3)$ 的左雅可比。

相邻时刻间的运动误差（用于构造代价项）：

$${\mathbf{e}}_{v,k}=\{\begin{array}{ll}\left[\begin{array}{c}-\ln (\mathbf{T}(t_0){\check{\mathbf{T}}}_0^{-1}{)}^{\vee }\\ {\check{\mathbf{\varpi }}}_0-\mathbf{\varpi }(t_0)\end{array}\right]& k=0\\ \left[\begin{array}{c}(t_k-t_{k-1})\mathbf{\varpi }(t_{k-1})-\ln (\mathbf{T}(t_k)\mathbf{T}(t_{k-1}{)}^{-1}{)}^{\vee }\\ \mathbf{\varpi }(t_{k-1})-\mathcal{J}{\left(\ln (\mathbf{T}(t_k)\mathbf{T}(t_{k-1}{)}^{-1}{)}^{\vee }\right)}^{-1}\mathbf{\varpi }(t_k)\end{array}\right]& k>0\end{array},\text{\hspace{1em}(11.9)}$$

代价项：

$$J_{v,k}=\frac{1}{2}{\mathbf{e}}_{v,k}^T{\mathbf{Q}}_k^{-1}{\mathbf{e}}_{v,k},{\mathbf{Q}}_k=\mathbf{Q}(t_k-t_{k-1}),\text{\hspace{1em}(11.10)}$$

总先验代价：

$$J_v=\sum _{k=0}^K{J}_{v,k}.\text{\hspace{1em}(11.11)}$$

物理意义：误差的上半部分衡量”从 $t_{k-1}$ 以当前速度匀速飞行到 $t_k$“与”实际位姿变化”之间的差异；下半部分衡量速度不连续性。两个分量都被 ${\mathbf{Q}}_k^{-1}$ 加权——时间间隔越大，权重越小（允许更大偏差）。

---

English

Converting back to global variables $\{\mathbf{T}(t_k),\mathbf{\varpi }(t_k)\}$, the motion prior error between consecutive states is:

$${\mathbf{e}}_{v,k}=\left[\begin{array}{c}\Delta t_k\mathbf{\varpi }(t_{k-1})-\ln (\mathbf{T}(t_k)\mathbf{T}(t_{k-1}{)}^{-1}{)}^{\vee }\\ \mathbf{\varpi }(t_{k-1})-\mathcal{J}(\cdots {)}^{-1}\mathbf{\varpi }(t_k)\end{array}\right],k>0.$$

The upper half penalizes deviation from constant-velocity motion; the lower half penalizes velocity discontinuity. The cost $J_{v,k}=\frac{1}{2}{\mathbf{e}}_{v,k}^T{\mathbf{Q}}_k^{-1}{\mathbf{e}}_{v,k}$ forms a factor-graph binary factor between $({\mathbf{T}}_{k-1},{\mathbf{\varpi }}_{k-1})$ and $({\mathbf{T}}_k,{\mathbf{\varpi }}_k)$.

---

11.1.4 线性化误差项 / Linearized Error Terms §

中文

为了在 GN 框架中使用，对误差项线性化。对位姿施加左扰动，对速度施加加性扰动：

$${\mathbf{T}}_k=\exp ({\mathbf{ϵ}}_k^{\wedge }){\mathbf{T}}_{\mathrm{op},k},{\mathbf{\varpi }}_k={\mathbf{\varpi }}_{\mathrm{op},k}+{\mathbf{\eta }}_k.\text{\hspace{1em}(11.12)}$$

联合扰动 ${\mathbf{\epsilon }}_k=[{\mathbf{ϵ}}_k^T,{\mathbf{\eta }}_k^T{]}^T$（维度 $12\times 1$）。

线性化后的误差：

$${\mathbf{e}}_{v,k}(\mathbf{x})\approx {\mathbf{e}}_{v,k}({\mathbf{x}}_{\mathrm{op}})+{\mathbf{F}}_{k-1}{\mathbf{\epsilon }}_{k-1}-{\mathbf{E}}_k{\mathbf{\epsilon }}_k,\text{\hspace{1em}(11.13)}$$

其中雅可比块为（$k>0$）：

$${\mathbf{F}}_{k-1}=\left[\begin{array}{cc}{\mathcal{J}}_{\mathrm{op},\mathrm{k},\mathrm{k}-1}^{-1}{\mathcal{T}}_{\mathrm{op},\mathrm{k},\mathrm{k}-1}& \Delta t_{k}1\\ \frac{1}{2}{\mathbf{\varpi }}_{\mathrm{op},k}^{⋏}{\mathcal{J}}_{\mathrm{op},\mathrm{k},\mathrm{k}-1}^{-1}{\mathcal{T}}_{\mathrm{op},\mathrm{k},\mathrm{k}-1}& 1\end{array}\right],\text{\hspace{1em}(11.14)}$$ $${\mathbf{E}}_k=\left[\begin{array}{cc}{\mathcal{J}}_{\mathrm{op},k,k-1}^{-1}& 0\\ \frac{1}{2}{\mathbf{\varpi }}_{\mathrm{op},k}^{⋏}{\mathcal{J}}_{\mathrm{op},k,k-1}^{-1}& {\mathcal{J}}_{\mathrm{op},k,k-1}^{-1}\end{array}\right],\text{\hspace{1em}(11.15)}$$

其中 ${\mathcal{T}}_{\mathrm{op},k,k-1}=\mathrm{Ad}({\mathbf{T}}_{\mathrm{op},k}{\mathbf{T}}_{\mathrm{op},k-1}^{-1})$，${\mathcal{J}}_{\mathrm{op},k,k-1}=\mathcal{J}(\ln ({\mathbf{T}}_{\mathrm{op},k}{\mathbf{T}}_{\mathrm{op},k-1}^{-1}{)}^{\vee })$。

将所有误差和雅可比堆叠，得到运动先验代价的二次近似：

$$J_v\approx \frac{1}{2}{\left({\mathbf{e}}_v({\mathbf{x}}_{\mathrm{op}})-{\mathbf{F}}^{-1}\delta {\mathbf{x}}_1\right)}^T{\mathbf{Q}}^{-1}\left({\mathbf{e}}_v({\mathbf{x}}_{\mathrm{op}})-{\mathbf{F}}^{-1}\delta {\mathbf{x}}_1\right),\text{\hspace{1em}(11.16)}$$

形式与第 9/10 章完全一致（${\mathbf{F}}^{-1}$ 是块三对角矩阵，$\delta {\mathbf{x}}_1$ 包含所有位姿和速度扰动）。

---

English

Perturbing poses via the Lie exponential and velocities additively, the linearized motion error is:

$${\mathbf{e}}_{v,k}(\mathbf{x})\approx {\mathbf{e}}_{v,k}({\mathbf{x}}_{\mathrm{op}})+{\mathbf{F}}_{k-1}{\mathbf{\epsilon }}_{k-1}-{\mathbf{E}}_k{\mathbf{\epsilon }}_k,$$

where ${\mathbf{\epsilon }}_k=[{\mathbf{ϵ}}_k^T,{\mathbf{\eta }}_k^T{]}^T$ is the $12\times 1$ joint perturbation (pose + velocity). The stacked quadratic form matches the structure of the discrete-time motion prior from Chapters 9-10, enabling the same Gauss-Newton framework.

---

11.2 STEAM：同步轨迹估计与建图 / STEAM: Simultaneous Trajectory Estimation and Mapping §

11.2.1 问题描述 / Problem Setup §

中文

STEAM 是连续时间版的 SLAM。待估状态包含位姿和广义速度：

$$\mathbf{x}=\{{\mathbf{T}}_0,{\mathbf{\varpi }}_0,{\mathbf{T}}_1,{\mathbf{\varpi }}_1,\dots ,{\mathbf{T}}_K,{\mathbf{\varpi }}_K,{\mathbf{p}}_1,\dots ,{\mathbf{p}}_M\}.\text{\hspace{1em}(11.17)}$$

与离散时间 SLAM 相比，每个时刻额外估计 ${\mathbf{\varpi }}_k$（$6\times 1$ 广义速度），这是连续时间先验所必须的 Markov 状态。

测量模型（与 SLAM 相同）：

$${\mathbf{y}}_{jk}=\mathbf{s}({\mathbf{T}}_k{\mathbf{p}}_j)+{\mathbf{n}}_{jk},{\mathbf{n}}_{jk}\sim \mathcal{N}(0,{\mathbf{R}}_{jk}).\text{\hspace{1em}(11.18)}$$

---

English

The STEAM state includes both pose and generalized velocity at each measurement time:

$$\mathbf{x}=\{{\mathbf{T}}_0,{\mathbf{\varpi }}_0,{\mathbf{T}}_1,{\mathbf{\varpi }}_1,\dots ,{\mathbf{T}}_K,{\mathbf{\varpi }}_K,{\mathbf{p}}_1,\dots ,{\mathbf{p}}_M\}.$$

The generalized velocity ${\mathbf{\varpi }}_k$ is the additional Markovian state required by the continuous-time prior. The measurement model is identical to discrete-time SLAM.

---

11.2.2 批量最大后验解 / Batch MAP Solution §

中文

完整 MAP 代价函数：

$$J(\mathbf{x})=J_v(\mathbf{x})+J_y(\mathbf{x}),\text{\hspace{1em}(11.19)}$$

其中 $J_v$ 是连续时间运动先验（第 11.1 节），$J_y$ 是视觉测量代价。

测量雅可比（注意与离散时间 SLAM 的细微区别）：

$${\mathbf{G}}_{1,jk}=\left[\begin{array}{cc}{\mathbf{S}}_{jk}({\mathbf{T}}_{\mathrm{op},k}{\mathbf{p}}_{\mathrm{op},j}{)}^{\odot }& 0\end{array}\right],{\mathbf{G}}_{2,jk}={\mathbf{S}}_{jk}{\mathbf{T}}_{\mathrm{op},k}\mathbf{D}.\text{\hspace{1em}(11.20)}$$

${\mathbf{G}}_{1,jk}$ 在速度扰动对应的列处填零，因为速度 ${\mathbf{\varpi }}_k$ 不直接参与路标测量。

全系统矩阵：

$$\mathbf{A}={\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{H},\mathbf{H}=\left[\begin{array}{cc}{\mathbf{F}}^{-1}& 0\\ {\mathbf{G}}_1& {\mathbf{G}}_2\end{array}\right],\mathbf{W}=\left[\begin{array}{cc}\mathbf{Q}& 0\\ 0& \mathbf{R}\end{array}\right].\text{\hspace{1em}(11.21)}$$

求解 $\mathbf{A}\delta {\mathbf{x}}^{\star }=\mathbf{b}$，更新工作点：

$${\mathbf{T}}_{\mathrm{op},k}\leftarrow \exp ({\mathbf{ϵ}}_k^{\star \wedge }){\mathbf{T}}_{\mathrm{op},k},{\mathbf{\varpi }}_{\mathrm{op},k}\leftarrow {\mathbf{\varpi }}_{\mathrm{op},k}+{\mathbf{\eta }}_k^{\star },{\mathbf{p}}_{\mathrm{op},j}\leftarrow {\mathbf{p}}_{\mathrm{op},j}+\mathbf{D}{\mathbf{\zeta }}_j^{\star },\text{\hspace{1em}(11.22)}$$

迭代至收敛。

---

English

The MAP cost function combines the continuous-time motion prior and visual measurement terms. The block ${\mathbf{G}}_{1,jk}$ is padded with zeros in the velocity columns since ${\mathbf{\varpi }}_k$ does not appear in the landmark measurement. The system has the same arrowhead structure as discrete-time SLAM, with ${\mathbf{A}}_{22}$ block-diagonal and ${\mathbf{A}}_{11}$ block-tridiagonal.

---

11.2.3 利用稀疏结构 / Exploiting Sparsity §

中文

连续时间运动先验不破坏箭头形稀疏结构：

$$\mathbf{A}=\left[\begin{array}{cc}\underset{{\mathbf{A}}_{11},\text{块三对角}}{\underbrace{{\mathbf{F}}^{-T}{\mathbf{Q}}^{-1}{\mathbf{F}}^{-1}+{\mathbf{G}}_1^T{\mathbf{R}}^{-1}{\mathbf{G}}_1}}& {\mathbf{G}}_1^T{\mathbf{R}}^{-1}{\mathbf{G}}_2\\ {\mathbf{G}}_2^T{\mathbf{R}}^{-1}{\mathbf{G}}_1& \underset{{\mathbf{A}}_{22},\text{块对角}}{\underbrace{{\mathbf{G}}_2^T{\mathbf{R}}^{-1}{\mathbf{G}}_2}}\end{array}\right].\text{\hspace{1em}(11.23)}$$

与离散时间 SLAM 的区别：$\delta {\mathbf{x}}_1$ 中每个时刻贡献 12 维（位姿 6 维 + 速度 6 维，而非仅 6 维）；${\mathbf{G}}_{1,jk}$ 矩阵有额外零列。其余稀疏利用方法（Schur/Cholesky）完全相同。

---

English

The structure is identical to discrete-time SLAM except each pose-slot in $\delta {\mathbf{x}}_1$ has $12$ dimensions (pose + velocity). ${\mathbf{A}}_{22}$ remains block-diagonal; ${\mathbf{A}}_{11}$ remains block-tridiagonal. The same Schur/Cholesky strategies apply.

---

11.3 插值与外推 / Interpolation and Extrapolation §

中文

关键优势：STEAM 求解出测量时刻的后验后，可在任意查询时刻 $\tau$ 以 $O(1)$ 代价插值/外推位姿和协方差。这是连续时间方法相对于离散时间方法最重要的优势。

11.3.1 均值插值 / Interpolating the Mean §

中文

假设 $t_k\le \tau <t_{k+1}$，只需两个相邻时刻 $t_k$ 和 $t_{k+1}$ 即可插值（Markov 性质）。

利用第 3.4 节的 GP 插值公式，后验插值方程化简为：

$${\hat{\mathbf{\gamma }}}_k(\tau )=\mathbf{\Lambda }(\tau ){\hat{\mathbf{\gamma }}}_k(t_k)+\mathbf{\Psi }(\tau ){\hat{\mathbf{\gamma }}}_k(t_{k+1}),\text{\hspace{1em}(11.24)}$$

其中插值核为：

$$\mathbf{\Lambda }(\tau )=\Phi (\tau ,t_k)-{\mathbf{Q}}_{\tau }\Phi (t_{k+1},\tau {)}^T{\mathbf{Q}}_{k+1}^{-1}\Phi (t_{k+1},t_k),\text{\hspace{1em}(11.25a)}$$ $$\mathbf{\Psi }(\tau )={\mathbf{Q}}_{\tau }\Phi (t_{k+1},\tau {)}^T{\mathbf{Q}}_{k+1}^{-1},\text{\hspace{1em}(11.25b)}$$

${\mathbf{Q}}_{\tau }=\mathbf{Q}(\tau -t_k)$。

局部状态 ${\hat{\mathbf{\gamma }}}_k(t)$ 包含局部位姿 ${\mathbf{\xi }}_k(t)$ 和局部速度 ${\mathbf{\psi }}_k(t)$，转换回全局变量：

$$\hat{\mathbf{T}}(\tau )=\exp \left({\left({\mathbf{\Lambda }}_1(\tau ){\hat{\mathbf{\gamma }}}_k(t_k)+{\mathbf{\Psi }}_1(\tau ){\hat{\mathbf{\gamma }}}_k(t_{k+1})\right)}^{\wedge }\right){\hat{\mathbf{T}}}_k,\text{\hspace{1em}(11.26a)}$$ $$\hat{\mathbf{\varpi }}(\tau )=\mathcal{J}\left(\ln (\hat{\mathbf{T}}(\tau ){\hat{\mathbf{T}}}_k^{-1}{)}^{\vee }\right)\left({\mathbf{\Lambda }}_2(\tau ){\hat{\mathbf{\gamma }}}_k(t_k)+{\mathbf{\Psi }}_2(\tau ){\hat{\mathbf{\gamma }}}_k(t_{k+1})\right).\text{\hspace{1em}(11.26b)}$$

计算代价 $O(1)$，与轨迹长度无关（仅用两个相邻时刻）。

---

English

Given the Markov property, interpolation at $\tau \in [t_k,t_{k+1})$ only requires the two bracketing states. The posterior interpolation equations reduce to:

$${\hat{\mathbf{\gamma }}}_k(\tau )=\mathbf{\Lambda }(\tau ){\hat{\mathbf{\gamma }}}_k(t_k)+\mathbf{\Psi }(\tau ){\hat{\mathbf{\gamma }}}_k(t_{k+1}),$$

with interpolation kernels $\mathbf{\Lambda }(\tau )$ and $\mathbf{\Psi }(\tau )$ computed from the GP transition and covariance functions. Converting to global variables gives $\hat{\mathbf{T}}(\tau )$ and $\hat{\mathbf{\varpi }}(\tau )$. Cost: $O(1)$ per query.

---

11.3.2 协方差插值 / Interpolating the Covariance §

中文

插值协方差需要更多工作。总体思路：

1. 提取主 MAP 解给出的 $t_k$ 和 $t_{k+1}$ 处的边缘协方差 $\hat{\mathbf{P}}(t_k,t_k)$、$\hat{\mathbf{P}}(t_k,t_{k+1})$ 等（利用 ${\hat{\mathbf{P}}}^{-1}=\mathbf{A}$ 的箭头形结构高效提取）。
2. 构造一个包含 $t_k$、$\tau$、$t_{k+1}$ 三个时刻的小型估计问题（图 11.6）：
   • 加入从 $t_k$ 到 $\tau$，再从 $\tau$ 到 $t_{k+1}$ 的新运动先验（两段）；
   • 减去从 $t_k$ 到 $t_{k+1}$ 的原始运动先验（一段）。
3. 对三时刻联合逆协方差 ${\hat{\mathbf{P}}}_{\mathrm{interp}}^{-1}$ 边缘化掉 $t_k$ 和 $t_{k+1}$ 处的状态，即可得到 $\tau$ 处的协方差 $\hat{\mathbf{P}}(\tau ,\tau )$。

最终结果（经 SMW 恒等式化简后）：

$$\hat{\mathbf{P}}(\tau ,\tau {)}^{-1}={\mathbf{E}}_{\tau ,k}^T{\mathbf{Q}}_{\tau ,k}^{-1}{\mathbf{E}}_{\tau ,k}+{\mathbf{F}}_{k+1,\tau }^T{\mathbf{Q}}_{k+1,\tau }^{-1}{\mathbf{F}}_{k+1,\tau }-(\text{修正项}),\text{\hspace{1em}(11.27)}$$

计算代价 $O(1)$，$\hat{\mathbf{P}}(\tau ,\tau )$ 在 $[t_k,t_{k+1}]$ 上平滑变化。

---

English

Covariance interpolation constructs a small three-time estimation problem: extract the marginal covariance at $t_k$ and $t_{k+1}$ from the main solution, add two new motion-prior factors connecting through $\tau$, remove the original factor spanning $[t_k,t_{k+1}]$, then marginalize out the bracketing times. The resulting formula for $\hat{\mathbf{P}}(\tau ,\tau {)}^{-1}$ involves only the transition matrices and covariances between $t_k$, $\tau$, $t_{k+1}$. Cost: $O(1)$ per query.

---

11.3.3 外推 / Extrapolation §

中文

在最后时刻 $t_K$ 之后外推（$\tau >t_K$）更简单：沿用常速假设。

均值外推：

$$\hat{\mathbf{T}}(\tau )=\exp \left((\tau -t_K)\hat{\mathbf{\varpi }}(t_K{)}^{\wedge }\right)\hat{\mathbf{T}}(t_K),\text{\hspace{1em}(11.28a)}$$ $$\hat{\mathbf{\varpi }}(\tau )=\hat{\mathbf{\varpi }}(t_K).\text{\hspace{1em}(11.28b)}$$

协方差外推（SMW 化简后）：

$$\hat{\mathbf{P}}(\tau ,\tau )={\mathbf{E}}_{\tau ,K}^{-1}\left({\mathbf{F}}_{\tau ,K}\hat{\mathbf{P}}(t_K,t_K){\mathbf{F}}_{\tau ,K}^T+{\mathbf{Q}}_{\tau ,K}\right){\mathbf{E}}_{\tau ,K}^{-T},\text{\hspace{1em}(11.29)}$$

形如 EKF 的协方差传播（但适配 $SE(3)\times {\mathbb{R}}^6$ 状态空间）。

---

English

Mean extrapolation ($\tau >t_K$): assume constant velocity from the last state:

$$\hat{\mathbf{T}}(\tau )=\exp ((\tau -t_K)\hat{\mathbf{\varpi }}(t_K{)}^{\wedge })\hat{\mathbf{T}}(t_K),\hat{\mathbf{\varpi }}(\tau )=\hat{\mathbf{\varpi }}(t_K).$$

Covariance extrapolation (via SMW identity):

$$\hat{\mathbf{P}}(\tau ,\tau )={\mathbf{E}}_{\tau ,K}^{-1}\left({\mathbf{F}}_{\tau ,K}\hat{\mathbf{P}}(t_K,t_K){\mathbf{F}}_{\tau ,K}^T+{\mathbf{Q}}_{\tau ,K}\right){\mathbf{E}}_{\tau ,K}^{-T},$$

which has the same form as the EKF covariance prediction, adapted to $SE(3)\times {\mathbb{R}}^6$.

---

11.4 讨论与扩展 / Discussion and Extensions §

中文

与离散时间方法的比较

方面	离散时间 SLAM (Ch10)	连续时间 STEAM (Ch11)
状态空间	位姿序列 $\{{\mathbf{T}}_k\}$	位姿+速度 $\{{\mathbf{T}}_k,{\mathbf{\varpi }}_k\}$
查询时刻	仅测量时刻	任意时刻 $O(1)$ 查询
平滑性	无（测量驱动）	GP 先验驱动
运动先验	离散积分	连续 SDE
实现复杂度	较低	较高（需处理 $\mathcal{J}$ 等）

时间戳的三种角色

连续时间方法允许将以下三类时间戳解耦：

• 测量时刻：传感器观测发生的时刻（通常很多）
• 估计时刻：MAP 求解的支撑节点（可以较少）
• 查询时刻：需要知道位姿的时刻（任意多）

解耦三类时间戳为高效实现提供了很大灵活性。

与第 10.1.5 节插值方法的比较

第 10.1.5 节的插值是基于约束的（强制常速假设）；本章的插值是基于惩罚的（鼓励平滑，但允许偏差，偏差由 $\mathbf{Q}$ 控制）。两种方法各有优劣：约束法严格但刚性，惩罚法灵活但需要调参。

扩展

• 白噪声加加速度（white-noise-on-jerk）：更高阶 SDE，支持更复杂运动模式。
• Singer 模型：相关加速度噪声模型，现实性更强。
• 连续体机器人：将”时间”换为”弧长”，同一框架适用于柔性梁的状态估计（Cosserat 杆先验）。
• 增量方法：结合 iSAM2 等增量求解器，实时处理新测量。

---

English

GP vs. constraint-based interpolation: The §10.1.5 interpolation enforces constant velocity as a hard constraint; STEAM uses GP smoothing as a soft prior, with smoothness controlled by $Q$. Both preserve arrowhead sparsity.

Three sets of timestamps: Continuous-time methods allow decoupling: (1) measurement times (dense), (2) estimation times (sparse support nodes), (3) query times (arbitrary). This flexibility enables efficient implementations where the estimation grid is coarser than the measurement rate.

Extensions: Higher-order SDEs (white-noise-on-jerk, Singer model) and incremental solvers (iSAM2-style updates) are natural extensions. The framework also applies to continuum robots, where arc-length replaces time and the Cosserat rod prior replaces the constant-velocity prior.

---

11.5 本章小结 / Chapter Summary §

中文

组件	功能	数学工具
局部 SDE	非线性 $SE(3)$ 动力学 → 局部线性	局部变量 ${\mathbf{\xi }}_k(t)$，${\mathcal{J}}^{-1}$
GP 先验	轨迹平滑	$\Phi (\Delta t)$，$\mathbf{Q}(\Delta t)$
线性化误差	GN 迭代所需	${\mathbf{F}}_{k-1}$，${\mathbf{E}}_k$（$12\times 12$）
MAP 求解	离散时刻估计	同 SLAM，保留稀疏结构
GP 插值	任意时刻均值	$\mathbf{\Lambda }(\tau )$，$\mathbf{\Psi }(\tau )$，$O(1)$
GP 协方差插值	任意时刻不确定性	3 时刻小估计问题，$O(1)$
外推	预测未来状态	常速外推 + 协方差传播

STEAM = 连续时间运动先验 + SE(3) MAP 优化 + GP 插值

---

English

Component	Role	Math tool
Local SDE	Nonlinear $SE(3)$ → linear local	Local vars ${\mathbf{\xi }}_k(t)$, ${\mathcal{J}}^{-1}$
GP prior	Trajectory smoothness	$\Phi (\Delta t)$, $\mathbf{Q}(\Delta t)$
Linearized errors	GN iterations	${\mathbf{F}}_{k-1}$, ${\mathbf{E}}_k$ ($12\times 12$)
MAP solve	Discrete-time estimates	Same as SLAM, sparse structure preserved
GP interpolation	Query mean at $\tau$	$\mathbf{\Lambda }(\tau )$, $\mathbf{\Psi }(\tau )$, $O(1)$
Covariance interp.	Query uncertainty at $\tau$	3-time sub-problem, $O(1)$
Extrapolation	Predict future states	Constant velocity + covariance propagation

STEAM = continuous-time GP prior + $SE(3)$ MAP optimization + GP interpolation/extrapolation

---

附录将提供本书使用的矩阵代数速查。/ The appendix provides a matrix algebra and calculus reference used throughout the book.