02.1｜先让每个数字有物理意义：传感器、坐标、单位与时间

学习问题：为什么三种传感器都在输出，位置仍会漂？本课先不写卡尔曼滤波。输入是带校验、状态与时间信息的 IMU、DVL、压力样本；输出是能放进同一运动模型的比力、角速度、体轴速度和深度观测。滤波器不能修复一个被当成米的毫米，也不能把接收时刻自动变成采样时刻。先建立数据契约，再讨论最优融合。

符号／对象	含义与维度	单位与坐标
$n,b,R_b^n$	NED、FRD、机体到导航系旋转	$3\times3$ 旋转矩阵，无量纲
$p^n,v^n,a^n$	IMU参考点位置、速度、运动加速度	m、m/s、m/s²；导航系
$f_m^b,\omega_m^b$	比力与角速度读数	m/s²、rad/s；机体系
$b_a,b_g,n_a,n_g$	两种偏置与两种测量噪声	对应加速度与角速度单位；机体系
$\pi,\pi_0,\rho,d$	压力、水面参考压力、密度、深度	Pa、Pa、kg/m³、m
$t_s,t_r,t_f,\Delta t$	采样、接收、融合时刻及区间长度	s，同一单调时钟

需要补的数学只读这些： 向量、单位与坐标和积分为何累积变化。先能口头解释本页小例，再进入完整推导；不必预先背下矩阵公式。

零基础先看：读数不是状态，状态也不是目标

“状态”是描述当前运动、足以继续预测的一组量，例如位置和速度；“观测”是传感器实际提供的读数；“目标”是我们希望以后达到的运动。压力读数不是目标深度，DVL速度也不是滤波器必须照单全收的真值。估计器回答“现在可能在哪里、走多快”，控制器才回答“为了到目标，下一步该推多少”。

先把三种容易混淆的误差放进同一静止实验：

现象	它改变了什么	能否靠多次平均消掉
加速度读数在零附近随机上下跳	测量噪声：本次读数有随机误差	独立、零均值条件下，平均能减小方差
每次都多读 $0.01\,\mathrm{m/s^2}$	传感器偏置：读数的中心错了	不能；平均后仍多读这么多
海流把艇真正推得加速	物理扰动：真实运动发生变化	不能把真实运动当噪声删掉

先算后滤。 恒定加速度偏置 $b=0.01\,\mathrm{m/s^2}$ 积分 $t=10\,\mathrm s$ ，会产生虚假速度 $bt=0.1\,\mathrm{m/s}$ 与虚假位移 $bt^2/2=0.5\,\mathrm m$ 。这里的平方时间来自“速度误差也在不断增大”，不是一个神秘滤波效应。这个例子解释了为什么后面要把偏置加入状态，并给不确定度单独记账。

微练习： 压力传感器每次都高读同一个值，而其随机抖动已经很小，调小观测方差会不会修复深度偏差？先写出理由，再展开。

概念答案：精密不等于准确

不会。小方差只表示围绕读数中心的随机散布小；中心本身可能错。调小方差反而会让滤波器更相信有偏读数。需要校准参考压力或显式建立偏置模型，并检查偏置是否能被其他信息区分。

本章动手路线：标量权衡 → 位置速度传播 → 对齐观测时刻。本课先完成单位与时间契约，后两课再解释它们为什么影响滤波。

从比力的定义推回运动加速度

假设作业区域足够小，使用局部平直 NED；忽略地球自转和运输角速度；重力大小 $g$ 在本段任务中已知且固定。加速度计内部的敏感质量感受支撑与推进等非重力作用，所以它测的是单位质量的非重力合力，即比力。自由落体时理想读数接近零，静止时反而读到抵消重力的支撑比力。偏置在短时间内缓慢变化，不是每次可独立平均掉的白噪声。

第一步：为什么加速度计静止时不读零？

要估计位置，我们需要的是运动加速度 $a^n=\dot v^n$ ；IMU提供的却是比力 $f_m^b$ 。这两个量的差别不是滤波参数，而是测量原理。把物体的单位质量受力分为重力 $g^n$ 与其余力，牛顿定律给出 $a^n=g^n+f^n$ ，所以 $f^n=a^n-g^n$ 。再把导航系的比力转回机体系，才能和传感器读数比较。

本式量	在做什么	尺寸与单位
$g^n=[0,0,g]^T$	NED向下的重力向量	$3\times1$ ， $\mathrm{m/s^2}$
$R_b^n$	把机体系坐标变为导航系坐标	$3\times3$ ，无量纲
$(R_b^n)^T$	执行相反的坐标变换，不是另一个姿态	$3\times3$
$b_a,n_a$	加速度偏置和零均值随机误差	$3\times1$ ， $\mathrm{m/s^2}$ ，机体系

因此测量方程是

f_m^b=(R_b^n)^T(a^n-g^n)+b_a+n_a.

我们要解出左侧读数所对应的 $a^n$ 。先把偏置与噪声移到左侧，再左乘 $R_b^n$ ；使用 $R_b^n(R_b^n)^T=I$ ，得到

R_b^n(f_m^b-b_a-n_a)=a^n-g^n.

最后加回重力，才得到可积分的运动加速度：

a^n=R_b^n(f_m^b-b_a-n_a)+g^n.

检查一个不依赖滤波器的物理场景：水平静止时 $R_b^n=I$ 、 $a^n=0$ ，因此理想读数必须是 $f^b=[0,0,-9.81]^T$ 。若水平静止程序算出非零 $a^n$ ，先排查符号、安装方向和单位，不要先调协方差。

第二步：把加速度变成下一时刻状态

现在的问题是：已知时刻 $k$ 的位置、速度、姿态和这一小段IMU读数，怎样推算 $k+1$ ？记 $\Delta t=t_{k+1}-t_k$ ，单位秒。帽子表示估计。先用估计偏置去掉可解释的固定误差，噪声均值取零：

\hat a_k^n=\hat R_k(f_{m,k}^b-\hat b_{a,k})+g^n.

这不是“忽略噪声影响”：均值方程不加入随机的具体实现值，其影响会在 $P$ 中增长。短区间内冻结 $\hat a_k^n$ ，积分 $\dot v=a$ 得到

\hat v_{k+1}^n=\hat v_k^n+\hat a_k^n\Delta t.

位置不能用已经更新后的速度直接乘整段时间，否则把加速效应算大一倍。积分区间内的速度 $\hat v(t_k+\tau)=\hat v_k+\hat a_k\tau$ ，有

\hat p_{k+1}^n=\hat p_k^n+\int_0^{\Delta t}(\hat v_k^n+\hat a_k^n\tau)d\tau =\hat p_k^n+\hat v_k^n\Delta t+\tfrac12\hat a_k^n\Delta t^2.

例如 $v_k=0.2\ \mathrm{m/s}$ 、 $a_k=0.1\ \mathrm{m/s^2}$ 、 $\Delta t=0.1\ \mathrm{s}$ ，新速度为 $0.21\ \mathrm{m/s}$ ，位移为 $0.0205\ \mathrm m$ ，而不是 $0.021\ \mathrm m$ 。

姿态也要推进。先定义去偏角速度 $\hat\omega_k=\omega_{m,k}^b-\hat b_{g,k}$ ，单位 $\mathrm{rad/s}$ ；乘时间后是旋转向量，方向为旋转轴、模长为转角。体轴角速度作用在姿态右侧：

\hat R_{k+1}=\hat R_k\operatorname{Exp}([\hat\omega_k\Delta t]_\times).

$[a]_\times$ 是满足 $[a]_\times b=a\times b$ 的反对称矩阵， $\operatorname{Exp}$ 将它映射成旋转矩阵。即便名义姿态转了很大角度，旋转矩阵仍满足正交约束；后续ESKF的小量只是它附近的误差，不要求名义姿态本身接近零。代码应先保存旧速度、计算位置增量，再更新速度，最后推进姿态，或一次性算出所有新量后一起赋值。

手算：静止不是零读数；压力也不是直接的位置

水平静止、偏置为零时，取 $g=9.81\,\mathrm{m/s^2}$ ，IMU理想读数为 $[0,0,-9.81]^T\,\mathrm{m/s^2}$ 。加回导航系重力后加速度恰为零。若错误地漏掉重力补偿，积分1 s会产生9.81 m/s的虚假速度。另一个更隐蔽的错误是姿态倾斜1°：小角近似把约0.171 m/s²重力投到水平轴，1 s后速度误差约0.171 m/s，位置误差约0.086 m。

第三步：压力转深度，还要把误差单位一起转换

压力测量的物理问题是“水柱有多高”，不是“当前垂直速度多大”。静水近似给出 $\pi-\pi_0=\rho gd$ 。 $\pi$ 是传感器压力， $\pi_0$ 是同一参考水面压力，两者单位Pa； $\rho$ 是水密度，单位 $\mathrm{kg/m^3}$ ； $d$ 是向下为正的深度，单位m。除以每米水柱压强 $\rho g$ 得到

d=\frac{\pi-\pi_0}{\rho g}.

例如表压 $19620\ \mathrm{Pa}$ ， $\rho=1000\ \mathrm{kg/m^3}$ ， $g=9.81\ \mathrm{m/s^2}$ ，结果为 $2\ \mathrm m$ 。如果后续观测向量以米为单位，它的噪声也必须以米描述。压力出现一个小误差 $\delta\pi$ 时，一阶泰勒展开给出

\delta d\approx\frac{\partial d}{\partial\pi}\delta\pi=\frac{\delta\pi}{\rho g}.

对两边平方取均值，假设压力误差零均值，便得到深度方差

\sigma_d^2\approx\frac{\sigma_\pi^2}{(\rho g)^2}.

取 $\sigma_\pi=98.1\ \mathrm{Pa}$ ，有 $\sigma_d=0.01\ \mathrm m$ ，应填入观测协方差的是 $R_d=10^{-4}\ \mathrm{m^2}$ ，不是 $0.01$ 。本式只传播随机压力误差；水面压力偏移、密度误差、动态压强会产生不同的模型误差。若深度已经修正到IMU参考点，则在15维误差状态中 $h_d(x)=e_3^Tp^n$ ， $H_d=[e_3^T\;0\;0\;0\;0]$ ，其中 $e_3=[0,0,1]^T$ ；若测压点有明显杆臂，应先包含其姿态引起的高度差。

一个样本从产生到融合经历什么

建议的离线数据结构至少保存 value、frame、unit、sample_time、receive_time、sequence、valid、quality。顺序是完整帧与校验检查、字段范围检查、安装变换、单位换算、设备时钟到主机单调时钟映射、有效性状态机、按样本时间入队。序号能发现重复帧；新收到不代表新采样。若硬件提供设备时间，可用时钟偏移与漂移模型映射；只有接收时间时，固定延迟与抖动必须分开处理。

第四步：什么时候的数值，必须在什么时候比较

设备时间和主机时间不一定来自同一块晶振。用一个短时间内近似固定的映射，把设备采样时间转换到主机单调时间轴：

t_s^{\mathrm{host}}=\alpha t_s^{\mathrm{device}}+\beta.

$\alpha$ 是无量纲的时钟速率比， $\beta$ 是秒制偏移。只有时钟偏移时可取 $\alpha=1$ ；长期运行还需要考虑漂移。这个映射只解决“两个钟怎么对齐”，没有解决“这个输出平均了哪一段时间”。

若设备给出区间末尾 $t_{\mathrm{end}}$ 和区间长度 $\Delta t$ ，而输出是区间平均速度，那么它自然对应区间中点：

t_{\mathrm{mid}}=t_{\mathrm{end}}-\frac{\Delta t}{2}.

例如一帧平均了 $9.90$ 到 $10.00\ \mathrm s$ 的速度，代表时刻应是 $9.95\ \mathrm s$ ，即使串口到 $10.03\ \mathrm s$ 才收齐也不改变这一事实。先统一设备时钟，再定义积分区间的代表时刻，最后记录接收延迟。若设备已经给出中点时间，就不要再减半个区间；时间修正只能做一次。

ArduSub代码阅读：把物理契约落实到边界

项目笔记给出的 IMU10 路径在 AP_InertialSensor_IMU10::publish_latest_sample 将0.01 g变为m/s²、0.01 deg/s变为rad/s并校正FRD方向；原始角速度100对应1°/s，即约0.01745 rad/s。压力路径是 AP_Baro_SerialModbus::update 的请求／等待／解析，接着 AP_Baro 压力转高度，再由 readBaroData 进入延迟缓存。DVL 的 ArduSub/handle_dvl_data.cpp BI接口按项目安装约定转换，而不是套用通用 ENU→NED。

第五步：DVL安装轴向与毫米单位分别处理

项目BI接口的原始三个分量不是直接的FRD米每秒。这里应分成两个操作：先用安装约定变号，再用 $10^{-3}$ 把 $\mathrm{mm/s}$ 换成 $\mathrm{m/s}$ ：

v_{\mathrm{BI}}^b=10^{-3}\begin{bmatrix}-1&0&0\\0&-1&0\\0&0&1\end{bmatrix} \begin{bmatrix}v_{x,\mathrm{raw}}\\v_{y,\mathrm{raw}}\\v_{z,\mathrm{raw}}\end{bmatrix}.

右侧原始向量单位为 $\mathrm{mm/s}$ ；左侧 $v_{\mathrm{BI}}^b$ 为机体FRD的 $\mathrm{m/s}$ 。变号矩阵解释安装朝向，不能因为最后单位正确就省去它。代入 $[-200,100,30]^T$ ，得到 $[0.20,-0.10,0.03]^T\ \mathrm{m/s}$ ，物理含义是前进、左移、略向下。进入导航系还需要 $v^n=R_b^nv^b$ ，不要把安装变换与运动姿态变换合并成一个来历不明的符号翻转。

原始 (−200,100,30) mm/s变成 (0.20,−0.10,0.03) m/s。EKF导航输出到 AP_InertialNav_NavEKF::update 后，控制接口部分使用 NEU 与 cm、cm/s；例如 NED 的向下速度+0.03 m/s，在NEU上轴是−3 cm/s。变号与乘100是两件操作，应在接口集中完成。

先独立手写：数据契约到一次预测

写一个输入原始BI三轴速度、压力Pa和样本区间的函数，返回FRD米每秒、深度米、深度方差和区间中点时间。
让水平静止样本连续预测100次，解释为何位置与速度应保持不变。
给加速度增加固定 $0.01\ \mathrm{m/s^2}$ 偏置，再积分10 s，先在纸上估计速度与位置误差，最后用程序验证。

不要从参考实现复制第一行开始。先写出每个函数参数的单位，并在调用处只做一次单位转换。

参考实现与运行检查（C++17，先完成练习再展开）

用 g++ -std=c++17 -O2 main.cpp -o main && ./main 编译运行。以下是离线教学实现，不连接传感器或执行器。

#include <array>
#include <cassert>
#include <cmath>
#include <iostream>
int main() {
    using V = std::array<double, 3>;
    V raw{-200,100,30};                 // mm/s，BI接口原始轴
    V vb{-raw[0]/1000,-raw[1]/1000,raw[2]/1000};
    V fm{0,0,-9.81}, gravity{0,0,9.81}; // 水平静止，R=I
    V acceleration{};
    for (int i=0;i<3;++i) acceleration[i]=fm[i]+gravity[i];
    const double depth=19620/(1000*9.81);
    const double sigma_depth=98.1/(1000*9.81);
    const double R_depth=sigma_depth*sigma_depth;
    assert(std::abs(depth-2)<1e-12 && std::abs(R_depth-1e-4)<1e-12);
    for(double a: acceleration) assert(std::abs(a)<1e-12);
    const double dt=.1, v_old=.2, a=.1;
    const double dp=v_old*dt+.5*a*dt*dt;
    const double v_new=v_old+a*dt;
    const double sample_mid=10.0-.5*dt;
    std::cout<<vb[0]<<' '<<vb[1]<<' '<<vb[2]<<'\n';
    std::cout<<depth<<' '<<R_depth<<' '<<dp<<' '<<v_new<<' '<<sample_mid<<'\n';
}

手写练习与精读路线

补充练习答案与阅读提示

练习：压力标准差98.1 Pa、密度1000时，写出深度标准差与方差；再说明平均窗口为何影响时间戳。答案要点：标准差0.01 m，方差0.0001 m²；窗口输出描述多个时刻，等权窗口自然代表其中心而非最新时刻。先读 Solà 的IMU测量模型，核对“比力—重力—坐标旋转”；再读官方EKF说明中延迟时域与输出预测器两段，为第4课建立直觉。TB02-S1 TB02-S2

TB02-S1 · Joan Solà｜Quaternion kinematics for the error-state Kalman filter

精读第5—6节的局部姿态误差、IMU传播、注入与重置；本章固定已知NED重力，使用15维误差而非文中的含重力扩展。

TB02-S2 · ArduPilot｜EKF2 Estimation System

重点读 How does it achieve this：延迟融合时域与当前输出预测是两项不同工作；该页解释架构，不作为项目EKF3状态维数的依据。