传感器究竟测到了什么：DVL、GPS、USBL 与深度的观测建模

本课回答一个很实际的问题：接到一个串口数据包以后，究竟把什么传给滤波器？为什么有时是三个数，有时是四条波束，有时又是一组卫星距离？

先读 02.6｜融合机制与位置跳变处理，再带着“测量是状态的某种函数，而不是完整状态本身”阅读本课。这里的物理模型和逐步代数推导用于建立理解；协议字段以设备实际型号、固件与配置为准。

1. 先分清四种不同的“维度”

1.1 IMU 的六个输出，不等于十五维状态

常见六轴 IMU 一次输出三轴角速度 $\omega_m\in\mathbb R^3$ 与三轴比力 $f_m\in\mathbb R^3$。单位分别为 $\mathrm{rad/s}$ 与 $\mathrm{m/s^2}$。比力是非重力力造成的单位质量加速度，不是已经去掉姿态影响的世界系运动加速度；静止放在桌面上也不应期待三轴加速度计都为零。

惯导维护的名义状态通常是

位置 $p^n$、速度 $v^n$、加速度计偏置 $b_a$、陀螺仪偏置 $b_g$ 各有三个数字；单位四元数 $q_{nb}$ 存四个数字，但受单位长度约束，仅有三个旋转自由度。因此名义状态存储 $3+3+4+3+3=16$ 个数字，而误差状态是

“十五维”是这套误差状态的维数，不是 IMU 提供了十五个独立测量。 有的系统还估计海流、声速、尺度因子或时钟，状态维数会增加；十五不是滤波器必须遵守的固定数字。

1.2 一个传感器只需要解释自己测到的部分

统一写成

$z\in\mathbb R^m$ 是实际收到的测量，$h(x)$ 是“假如真实状态为 $x$，这个传感器应该读到什么”，$n$ 是测量噪声，$r$ 是实际与预测读数的差，$H$ 是对十五维误差的小扰动导数。

若本次有 $m$ 个观测分量，则

本课用 $\Sigma_z$ 表示测量噪声协方差，也就是常见 KF 公式中的 $R$，避免与旋转矩阵混淆。三维 DVL 速度给出 $m=3$，一条声学距离给出 $m=1$，四条有效 DVL 波束给出 $m=4$。改变的是观测行数，不是每收到一个不同的传感器就重建一套十五维状态。

2. 全课坐标、杆臂与误差约定

$n$ 为固定局部 NED 导航系，$b$ 为 IMU/机体系，$d$ 为 DVL 测量坐标系。$C_{nb}$ 将 $b$ 系向量变换到 $n$ 系，$C_{db}$ 将 $b$ 系向量变换到 $d$ 系。旋转矩阵没有长度单位。

$l_D^b$ 是从 IMU 原点指向 DVL 测量中心的安装向量，单位为 m。GPS 天线、USBL 应答器、压力口各有自己的杆臂，分别写作 $l_A^b,l_U^b,l_P^b$。状态位置统一定义为 IMU 原点的位置，不能这一帧估计 IMU、下一帧偷偷改成 GPS 天线。

姿态采用右侧小扰动：

$[a]_\times b=a\times b$，$\operatorname{Exp}$ 是把三维小旋转映射为旋转矩阵的指数映射。后面的雅可比符号依赖这个约定；换成左扰动时不能直接照抄。

3. DVL 为什么能够测速度？

3.1 一条波束首先测的是沿波束的速度

厂家说明 DVL 向海底发射倾斜声束，分析回波频移；由多个方向的回波获得速度。它不是像 GPS 那样直接读出绝对坐标。Water Linked：DVL 原理

为了推导，考虑最简单的单站往返声学模型：均匀静水、固定散射点、同一个换能器收发、传播路径近似不变，传感器沿着指向散射点的波束方向运动。定义：

• $f_0$：发射声频，单位 Hz；
• $c_w$：水中声速，单位 $\mathrm{m/s}$；
• $u$：传感器朝向散射点的速度投影，接近为正；
• $f_r$：传感器收到的回波频率。

第一程，声源向散射点靠近，波峰间距变小，散射点接收到的频率是

分母 $c_w-u$ 表示声源在发出相邻波峰之间追上了前面的波峰。第二程，散射点相当于返回频率为 $f_1$ 的声波，接收器迎着回波运动，于是

相减得到频移

ROV 的速度通常远小于声速，$|u|/c_w\ll1$，因此一阶近似为

分母中的 $2$ 来自去程和回程各一次多普勒作用。 这个近似不是所有声学设备通用的无条件公式；双基地几何、实际波束方向、设备频移正号定义都需要匹配。

一个手算例子：取 $f_0=600000\ \mathrm{Hz}$、$c_w=1500\ \mathrm{m/s}$，测得 $\Delta f=200\ \mathrm{Hz}$，则

这只是沿该波束的投影，不能直接宣称机器人前向速度就是 $0.25\ \mathrm{m/s}$。这个例子为教学自设，不代表某个设备规格。

3.2 为什么三维速度需要多个方向？

第 $i$ 条波束单位向量为 $b_i^d\in\mathbb R^3$，从换能器指向反射区域。经过频移符号与单位统一后，波束速度模型为

$v_D^d$ 是 DVL 测量中心的三维速度，$\epsilon_i$ 是该波束速度误差。点积只保留沿波束的分量：与波束垂直的运动对这一条观测没有一阶贡献。

把 $m$ 条有效波束叠起来：

$B$ 是 $m\times3$ 几何矩阵。设四束在与竖直方向夹角为 $\alpha$ 的理想对称布局上：

前两束相减，竖直分量抵消；相加，水平分量抵消。因此无噪声时

若 $\alpha=30^\circ$、$v_D^d=(1,0.2,0.1)^T\ \mathrm{m/s}$，四束读数约为

负波束速度不是设备坏了，而是沿该波束的投影为负。代回三个式子会还原原速度。

3.3 有噪声时为什么要最小二乘？

四个有噪声的读数通常不可能由同一个三维速度精确解释。我们选择让加权残差最小的速度：

$\Sigma_b$ 是波束速度噪声协方差；可信波束方差较小，权重较大。展开并对 $v$ 求导：

所以正规方程为

当 $B$ 满列秩、协方差正定时，理论解与其协方差为

实际 C++ 数值实现优先使用白化后的 QR/SVD 分解，不要显式求逆；构造 $B^TWB$ 还会放大条件数问题。满秩只说明可以解，不说明几何条件良好。

假设理想四束相互独立且每束方差都是 $\sigma_b^2$，代入上述对称几何可得

这揭示了布局与误差的关系：波束接近竖直时，$\sin\alpha$ 很小，水平测速噪声被放大；不能仅凭“有四条波束”就认为三个方向精度相同。

3.4 少了波束，究竟还能不能融合？

• 用波束先独立解三维速度，至少要有秩为 $3$ 的几何信息；“三束有效”还需检查几何独立性。
• 只有两束时，不能凭空独立求出三维速度。但可直接把两条波束作为两个标量观测送入已有惯导滤波器，更新它们直接约束的方向。它们没有为其他方向创造独立新约束，但其他状态仍可能通过先验交叉协方差得到间接校正。
• 后一种方案是利用先验的部分观测更新，不等于“两束已经测出完整三维速度”。

若三维 DVL 观测函数为 $h_D(x)$、雅可比为 $H_D$，则原始波束更新直接使用

$H_b$ 是 $m\times15$。丢一束就删除对应观测、几何矩阵行与噪声协方差行列，不要补一个零速度假装正常。使用伪造波束恢复值时，还必须说明它依赖哪些旧数据，避免把由同一 IMU 预测出来的速度当独立新证据重复融合。

4. DVL 安装位置与方向如何进入观测函数？

4.1 刚体上不同位置的速度不同

从 IMU 原点到 DVL 的向量是 $l_D^b$。导航系下 DVL 位置为

安装杆臂在机体系不变，对时间求导：

这里采用短程局部导航近似，忽略地球自转和导航坐标运输角速率；$\omega^b$ 表示相对于导航系的机体角速度。高精度长航时系统需要补齐地球项。第二项是旋转造成的切向速度，不能因为机器人中心不平移就忽略。

变换到 DVL 系得到底跟踪观测函数：

先把 IMU 原点的导航系速度旋回机体，再加旋转杆臂速度，最后转到设备实际报告的轴系。若设备已经按配置输出机体系速度，就不应再重复应用 $C_{db}$。

例：IMU 原点静止，$\omega^b=(0,0,0.2)^T\ \mathrm{rad/s}$，$l_D^b=(0.5,0,0)^T\ \mathrm m$，则

此时 DVL 报告 $0.1\ \mathrm{m/s}$ 横向速度完全可能是正常的。如果硬认为中心在横移，滤波器就会在转弯时错误修改速度。

4.2 逐块求 DVL 雅可比

令 $w=\hat C_{nb}^T\hat v^n$。由右扰动近似：

最后一步用了 $a\times b=-b\times a$，这正是姿态块出现正号的原因。

角速度去偏置项的误差是 $-\delta b_g$，杆臂项变化为

因此在本文十五维顺序下

这不是“DVL 只更新速度”的矩阵：机体系速度读数也受姿态影响；有杆臂时还受角速度偏置影响。是否真能稳定估出这些量，还取决于运动激励和其他观测，不能只看某个雅可比块非零。

这里把陀螺仪读数作为已知输入写进 $h_D$。其噪声通过杆臂进入观测残差，而且同一陀螺仪也参与惯导预测，可能产生过程噪声与观测噪声相关性。严谨实现需建模这种相关性；在杆臂很短、噪声影响较小时才能把忽略项作为经过量级评估的近似，而不是永远假设完全独立。

4.3 底跟踪和水跟踪不是同一种速度

底跟踪近似测相对静止海底的速度；水跟踪测相对散射水层的速度。厂家明确将两类报告区分，水跟踪不能直接当作对地速度。Water Linked：Water tracking

如果局部水流在导航系速度为 $v_c^n$，水跟踪模型应改成

例如机器人相对地面静止，水向东流 $0.3\ \mathrm{m/s}$，设备可能测到相对水向西 $0.3\ \mathrm{m/s}$。如果当成对地速度积分，系统会凭空积累位置漂移。

需要估计未知三维海流时，可以增广 $v_c^n$，误差状态由 $15$ 维变成 $18$ 维，并添加流速过程模型；水跟踪的海流观测块是 $-C_{db}C_{nb}^T$。仅凭相对水速度不能唯一分开机器人地速与流速，必须有其他绝对参考、运动约束或可信先验。

4.4 设备给出的其他字段怎么用？

以 Water Linked A50/A125 官方协议为具体例子，它区分三维速度、速度协方差、各波束有效性、测速有效性、测量有效时刻与发送时刻；速度单位为 $\mathrm{m/s}$，协方差单位为 $(\mathrm{m/s})^2$。官方 TCP JSON 协议

在自己的驱动中逐项核对，而不是只解析三个浮点数：

1. 速度已经在哪个坐标系？是否在设备里配置过安装旋转？
2. 三轴速度是否同一测量时刻？时间字段是采样、回波中心、发送还是主机接收？
3. 协方差是否包含非对角项？不要在旋转轴系后仍沿用旧协方差。
4. 有效标志、波束数、质量量、底/水跟踪模式是否一致？
5. 设备内部是否融合了 IMU，输出的是原始速度还是内部滤波结果？其位置输出是否只是该速度的积分？

同一 DVL 的积分位置和速度高度相关，不能不加说明地当成两份独立测量来“多融合一次”。如果这些速度已经被当前滤波器吸收，后来输入的积分位置还可能与当前先验误差相关；仅在新观测的噪声矩阵中增加两个分量的相关项，并不自动解决这种先验与观测的相关性。“质量分数”不是天然的方差，若要映射成 $\Sigma_z$，需要协议定义或用残差统计标定。

5. GPS/GNSS：经纬度输出与原始卫星观测是两层东西

5.1 GPS 不直接测一个三维位置点

接收机首先获得卫星信号的码传播时间、载波相位及多普勒信息，再结合卫星轨道、时钟与误差模型计算导航解。伪距包含时钟误差，因此不是纯几何距离；多普勒与相对径向速度及接收机频率误差有关。ESA Navipedia：GNSS Basic Observables

工程中分两种接口：

• 解算输出：经纬高、NED/ENU 速度、定位状态和协方差。这适合先学习松耦合。
• 原始观测：逐卫星伪距、载波、多普勒、星历与时间。这用于紧耦合，需要更多误差模型和状态。

水下无线电卫星信号通常不可用；ROV 的 GPS 常在水面阶段使用，或者用于确定水面 USBL 基站的位置，不应把水面船的 GPS 当作水下机器人的 GPS。Krauss 与 Stilwell，OCEANS 2022

5.2 为什么不能把经纬度直接减去米制位置？

纬度 $\varphi$、经度 $\lambda$ 的单位为 rad，高度 $h$ 为 m，而滤波器位置一般是局部米制坐标。必须先统一坐标。

给定参考椭球长半轴 $a$、第一偏心率平方 $e^2$，卯酉圈曲率半径为

地理坐标转 ECEF：

其中 $p^e$ 是地心地固坐标，单位 m，$h$ 为椭球高。海拔/正高与椭球高不是同一个量，深度更不能直接拿来替代它。

选择固定局部原点 $p_0^e$，其纬经度为 $(\varphi_0,\lambda_0)$。从 ECEF 位移转到 NED 的矩阵为

三行分别取向北、向东、向下的投影，局部位置是

减原点给出位移，再旋转到所需轴系。官方 ENU 变换通过交换前两轴、翻转 Up 轴即可得到这里的 NED 形式。ESA：ECEF 与 ENU 变换

当原点与旋转视作确定量时，协方差一起变换：

若输入协方差是经纬高协方差，则需要地理坐标到 ECEF 的雅可比 $J_{e,\mathrm{llh}}$：

$J$ 的前两列把弧度误差映射为米，不能把“经度方差 $10^{-10}$”当成“位置方差 $10^{-10}\ \mathrm{m^2}$”。若原点本身由一次有噪 GPS fix 设置，其不确定性及与当前 fix 的相关性也应计入；不能凭换了局部原点就创造一个无误差绝对基准。

5.3 GPS 位置更新的杆臂与雅可比

GPS 测的是天线位置，而状态定义在 IMU：

用右扰动展开：

所以

尺寸为 $3\times15$。无杆臂时姿态直接块为零，但通过协方差传播仍可能有间接校正。GNSS 若提供独立速度解，也要用天线点速度模型；若同时使用位置与速度，观测相关性不能自动当成零。

5.4 紧耦合为什么需要时钟状态？

为单独推导，采用 ECEF 坐标。第 $j$ 颗卫星位置为 $s_j^e$，天线位置为 $p_A^e$，几何距离为

把卫星时钟及大气等已知改正处理后，一个教学用码伪距模型为

$b_c=c_0\delta t_r$ 是接收机时钟偏差乘真空光速 $c_0$，单位 m；没有把这项建模，就会把时钟误差误认为位置误差。实际还要处理卫星发射时刻、地球自转、传播与多路径残差，本式是说明状态结构的简化。

令 $u_j=(s_j^e-\hat p_A^e)/\hat\rho_j$ 为天线指向卫星的单位视线。微分距离：

负号有直观意义：天线沿着卫星方向移动，距离减小。若 $C_{en}=T_{ne}^T$，则

因此该卫星伪距的雅可比行中，位置块为 $-u_j^TC_{en}$、姿态块为 $u_j^TC_{en}\hat C_{nb}[l_A^b]_\times$，时钟偏差块为 $1$。

用伪距率表示多普勒观测时，教学模型为

$v_{s,j}^e$、$v_A^e$ 分别是卫星和天线速度；$\dot b_c$ 为以 $\mathrm{m/s}$ 表示的时钟漂移。设备若输出多普勒频率，需要按其符号约定乘波长转换，不能默认所有接收机都使用同一正号。视线本身依赖位置，完整雅可比还包含这部分导数。

若估计 $b_c$ 和 $\dot b_c$，十五维惯导误差状态变成十七维。不是因为卫星有多少颗就把状态变成多少维；卫星数量决定本次观测有多少行，时钟等未知量才决定状态新增多少列。

6. USBL：声学距离与方向怎样变成位置？

6.1 从往返传播时间得到斜距

常见 USBL 系统由水面换能器阵列与水下应答器组成。系统结合声学往返时间及阵列方向信息估计相对位置。Sonardyne：USBL 工作原理

设测得总往返时间为 $T$，应答器已知回复等待时间为 $\tau_r$，水中有效声速为 $c_w$。忽略往返过程中的运动、折射及声速空间变化时：

$T-\tau_r$ 是传播时间，除以二才是一程斜距。例如 $T=0.210\ \mathrm s$、$\tau_r=0.010\ \mathrm s$、$c_w=1500\ \mathrm{m/s}$，斜距为 $150\ \mathrm m$。

对时间误差线性化，$\partial r/\partial T=c_w/2$。只考虑时间标准差 $\sigma_T$ 时，$\sigma_r\approx(c_w/2)\sigma_T$。但声速有误差时还需加上 $\partial r/\partial c_w=(T-\tau_r)/2$ 对应的不确定性；“时间测得准”不等于“距离必然准”。

6.2 到达时间差为什么能测方向？

令阵列中心到目标的单位方向为 $d$，阵元相对阵列中心位置为 $a_i$。目标在远场、局部波前近似平面时，第 $i$ 个阵元到目标的距离近似为 $r-a_i^Td$。两阵元的到达时间差为

差分消去了共同的发送时刻和公共传播距离，留下与阵元基线方向的投影。因此多个不同基线方向能提供到达方向信息。近场情况下应该保留精确距离差，而不是继续使用平面波近似。

若定义相位差为“相位延迟之差”，则 $\Delta\phi_{ij}=2\pi f\Delta t_{ij}$；若定义为接收信号瞬时相位 $\phi_i-\phi_j$，在相同信号约定下会带相反号，因此必须与协议定义对应。相位按 $2\pi$ 周期重复，便存在模糊性；不能把任意单频相位差都当唯一角度。实际系统需要阵列设计、信号处理与标定解决。

6.3 极坐标到相对直角坐标，误差会怎么变化？

定义阵列局部坐标：$x$ 向前、$y$ 向右、$z$ 向下；方位角为 $\alpha$，俯角 $\beta$ 从水平面向下为正。斜距与方向生成

对三个输入求导，得到极坐标到直角坐标的雅可比：

第一列无长度单位，后两列单位为 $\mathrm{m/rad}$。若极坐标噪声协方差为 $\Sigma_{r\alpha\beta}$，则

这解释了为什么 USBL 的位置协方差常常不是三个相同常数：角度误差导致的横向误差随距离增长。小角度下，距离 $r=100\ \mathrm m$、方向标准差 $0.5^\circ\approx0.00873\ \mathrm{rad}$，横向标准差量级约为 $r\sigma_\alpha=0.873\ \mathrm m$；到 $500\ \mathrm m$ 时变成约 $4.36\ \mathrm m$。这是几何误差传播示例，不是某款产品精度承诺。

6.4 相对船的位置如何变成共同导航系位置？

设水面阵列中心的导航系位置为 $p_S^n$，阵列系到导航系姿态为 $C_{ns}$，则水下应答器位置为

这一步依赖水面船的定位、船体姿态、阵列安装外参和声学有效时刻。如果船在动，使用“当前船位置”转换几秒前的声学观测就会造出假偏差。

若 $p_S^n$ 和 $C_{ns}$ 不是精确量，观测协方差不仅含声学部分 $C_{ns}\Sigma_s C_{ns}^T$，还要包含基站位置和姿态误差通过雅可比传播的贡献；若它们相关，还要保留交叉项。共同基站偏差会造成多帧 USBL 误差相关，不能仅因更新次数多就认为误差按独立白噪声无限缩小。

得到位置后，面向 ROV 的松耦合观测仍然是

它与 GPS 位置观测的几何形式相同，但噪声来源、时间延迟、有效性判据和可用环境不同。滤波器不关心传感器品牌，关心的是这三个数字在物理上测量了哪个点、哪个时刻、哪个坐标系的位置。

6.5 紧耦合并不等于“用了很多传感器”

对已知固定声学基站位置 $a^n$，ROV 应答器位置为 $p_U^n=p^n+C_{nb}l_U^b$。如果直接融合单程距离，则

令 $e=(\hat p_U^n-a^n)/\|\hat p_U^n-a^n\|$，则

这里只有一行：一次距离观测只能约束相应的径向方向，不能直接提供完整三维位置。运动、多个基站和其他传感器可以在时间上补充几何信息。

若直接融合往返时间，简化模型为 $h_T(x)=2h_r(x)/c_w+\tau_r$，对应 $H_T=2H_r/c_w$，残差单位变为 s，噪声协方差单位变为 $\mathrm{s^2}$。同一个物理约束用距离或时间表达都可以，但函数、雅可比和噪声单位必须一致。

Morgado 等人的成熟 USBL/INS 工作把声学往返时间和到达时间差直接嵌入 EKF，而不是先单独算出一个 USBL 位置再融合。其价值在于保留更原始的阵列几何和误差结构，不是紧耦合这个名字本身保证更优。JFR 2013 原论文记录；作者机构公开的早期技术论文

7. 压力计为什么主要约束深度？

压力口测到的是压力，不是天然的导航系 $z$ 坐标。在静水、密度近似常数时，竖直方向薄层水体的重量平衡给出

$d$ 表示从水面向下的深度，$\rho_w$ 为水密度，$g$ 为重力加速度大小。积分并取水面压力 $P_0$：

例如 $P-P_0=100000\ \mathrm{Pa}$、$\rho_w=1025\ \mathrm{kg/m^3}$、$g=9.81\ \mathrm{m/s^2}$，深度约为 $9.95\ \mathrm m$。改成淡水密度 $1000\ \mathrm{kg/m^3}$，同一压差对应约 $10.19\ \mathrm m$；密度设置错误会形成尺度误差。

厂家压力传感器资料也将压力测量与深度应用区分；高分辨率并不等于同等数值的绝对深度准确度。Blue Robotics 压力/深度传感器

若导航系 NED 的 Down 原点取在参考水面，令 $e_D=(0,0,1)^T$，深度模型为

$b_d$ 是可选深度零点偏差。若不估计它而假设已标定为零，对十五维误差状态：

尺寸为 $1\times15$。如果增广 $b_d$，需新增状态与一列 $1$。压力观测的噪声换算为深度方差时，仅考虑压力白噪声有 $\sigma_d^2\approx\sigma_P^2/(\rho_wg)^2$；水面基准、密度、温度补偿、压力口附近动压等是另外的误差来源。

8. 不同信息维度如何进入同一滤波器：一张实现表

为了理解尺寸，取某一时刻的 DVL 三维速度观测：

再到下一时刻，只收到深度：同一个 $P^-$ 仍然是 $15\times15$，但 $S$ 变成 $1\times1$，$K$ 变成 $15\times1$。状态不需要填补“另外十四个观测”；深度只能提供一条真实约束，其他误差方向按先验与相关性更新。

融合后的输出不是“一个新传感器数据包”，而是带共同时间和坐标定义的导航状态： 位置、速度、姿态、偏置估计、误差协方差以及健康状态。控制器通常消费位置、速度、姿态；日志和失效管理还需要时间、方差、新息与观测接受/拒绝原因。

9. 驱动接口要传的不止数值

下面是本课自己的教学接口，不是任何厂家或 ArduSub 的原文 API。其作用是把“几何、时刻、质量、相关性”变成代码里不能忘记的字段。

#include <array>
#include <cstdint>
#include <vector>

enum class Frame { NED, BODY_FRD, DVL_DEVICE, ECEF };
enum class Kind { DvlVelocity, DvlBeams, Position, Depth, AcousticRange };

struct Measurement {
    Kind kind;
    Frame frame;
    std::int64_t sample_time_us; // 测量有效时刻，已映射到融合时钟
    std::int64_t receive_time_us;// 收到数据的时刻，用于诊断延迟
    std::uint64_t sequence;      // 防止同一帧重复融合
    std::vector<double> z;       // m 个分量，类型决定单位
    std::vector<double> covariance; // m*m，行主序，与 z 同坐标；元素单位为对应两分量单位的乘积
    std::array<double, 3> lever_arm_body_m;
    bool valid;
    bool bottom_track;          // 对 DVL 解释必不可少
    std::uint32_t valid_mask;    // 按轴或按束，需要接口明确
    std::uint32_t source_id;     // 区分传感器及其共同数据来源
};

进入更新前至少检查：$z$ 与协方差维度一致、数据有限、协方差对称且有效、单位和轴系匹配、时钟转换已完成、同一序号未融合、当前跟踪模式对应正确模型。不要用“只要能解析出浮点数就送 EKF”代替这些检查。

9.1 一个可手写的四束几何恢复函数

下面代码仅实现本课已明确的理想四束对称布局，没有冒充通用 DVL 驱动。它用于练习投影关系；实际设备须使用标定波束矩阵及加权 QR/SVD。

#include <array>
#include <cmath>
#include <stdexcept>

std::array<double, 3> recoverJanus(
    const std::array<double, 4>& beam_mps, double angle_rad) {
    for (double x : beam_mps)
        if (!std::isfinite(x)) throw std::invalid_argument("invalid beam");
    const double s = std::sin(angle_rad);
    const double c = std::cos(angle_rad);
    if (!std::isfinite(angle_rad) || std::abs(s) < 1e-6 || std::abs(c) < 1e-6)
        throw std::invalid_argument("degenerate geometry");
    return {
        (beam_mps[0] - beam_mps[1]) / (2.0*s),
        (beam_mps[2] - beam_mps[3]) / (2.0*s),
        (beam_mps[0] + beam_mps[1] + beam_mps[2] + beam_mps[3]) / (4.0*c)
    };
}

练习：自行生成 $Bv$，验证零速度、纯前向、纯横向、纯竖直、混合运动及接近退化角度。再给第三束加异常值，观察对恢复的影响，并思考为什么需要残差与质量检查。

#include <cassert>
int main() {
    constexpr double pi = 3.14159265358979323846;
    const double a = pi/6.0, s = std::sin(a), c = std::cos(a);
    const std::array<std::array<double,3>,5> cases{{
        {0,0,0}, {1,0,0}, {0,1,0}, {0,0,1}, {1,0.2,0.1}
    }};
    for (auto v : cases) {
        std::array<double,4> y{{s*v[0]+c*v[2], -s*v[0]+c*v[2],
                                s*v[1]+c*v[2], -s*v[1]+c*v[2]}};
        auto out = recoverJanus(y,a);
        for (int j=0;j<3;++j) assert(std::abs(out[j]-v[j]) < 1e-12);
    }
    bool rejected=false;
    try { recoverJanus({0,0,0,0},0.0); }
    catch (const std::invalid_argument&) { rejected=true; }
    assert(rejected);
}

10. 对接 ArduSub 时，按这条链核对

1. 协议解析层：把整数或二进制字段解成物理量；毫米每秒要转换为米每秒，角度每秒要转换为弧度每秒，但也要核对目标 API 的实际单位，不能在两个层重复转换。
2. 传感器语义层：明确输出是原始波束、设备系速度、机体系速度、导航系速度还是厂家内部积分位置；保留有效性和时间。
3. 几何适配层：确认安装旋转在设备、桥接程序、飞控三处哪一处生效；杆臂在观测函数中补偿，不要凭经验把正负号试到“看起来稳定”为止。
4. 估计器入口层：查看实际使用的版本、传感器源选择与观测函数。收到 MAVLink 消息不等于该量已经参与 EKF 更新，更不等于量纲、轴系、延迟正确。
5. 创新与输出层：记录原始观测、预测观测、新息、新息方差、接受状态和输出状态。观察转弯时的速度残差是否与 $\omega\times l$ 同步，远距离 USBL 残差是否符合角度噪声放大趋势。

成熟工程的核心不是“选择哪个高级滤波器名称”，而是这条可追溯链：传感器实际测量的物理量 → 它与共同状态的关系 → 测量误差模型 → 时刻与坐标对齐 → 更新 → 对外发布可用状态。

11. 继续阅读与课堂自查

• 02.3｜十五维误差传播：从 IMU 运动学推导 $F,G$，区分预测模型与本课观测模型。
• 02.6｜融合机制与位置跳变处理：理解多个观测如何修改同一状态，位置与速度冲突时怎样处理。
• Water Linked DVL 协议：练习把字段、单位、时间戳与噪声矩阵对应起来。
• ESA GNSS 基本观测：区分码伪距、载波相位和多普勒。
• Morgado 等，JFR 2013：理解直接使用声学原始观测的紧耦合思路。

读完后应能不看答案解释：为什么单条距离只有一行 $H$？为什么姿态误差会影响机体系 DVL 速度？为什么 DVL 中心纯旋转也会有速度？为什么 GPS 原始观测要增加时钟而设备位置输出通常不用？为什么多个传感器观测维数不同仍能共用同一个 $15\times15$ 协方差？