2.3 导航显示：环境状态估计与建模
2.3 导航显示：环境状态估计与建模
本节摘要：状态估计是感知舱的心脏：从含噪量测与运动模型里，递推出对世界状态的当前信念。本节用一维卡尔曼滤波的可运行骨架讲透预测-更新循环，说明多源融合的互补、竞争、协作三种形态，给定位与建图（SLAM）挂接接口，并以估计一致性与发散排查收尾——仪表读数必须同时回答"是多少"和"有多大把握"。
阿波罗计划的登月舱里，导航计算机算力放今天不如一块家电遥控器，却要把飞船从地球送到月面再落下来。它的底牌正是本节的主角：卡尔曼滤波——用运动模型往前推、用量测往回拉，在两者之间按噪声大小动态分配信任。此后从潜艇导航到手机定位，这套"预测-更新"循环几乎统治了所有需要从噪声里挤真相的场合。上一节洗好的干净量测，在本节被织进信念：仪表读数从此不再是一瞬快照，而是带着历史与把握的判断。
问题定义：从量测流到信念流
状态估计要解决的不对称是：量测是瞬时的、带噪的、可能缺失的，而决策需要连续的、稳定的、带置信度的信念。雷达这一帧说障碍在三米处，下一帧被挡住没有回波——飞行器不能跟着"失明"半拍。估计器的解法是维护信念状态：每拍先按运动模型预测（"按我上一拍的估计与速度推，现在应该在三米二"），再用量测更新（"雷达说三米零，修正一点"）。量测在就信一点，量测缺就全靠推，推的代价是不确定性随时间增长。
贝叶斯滤波是这个思想的通用形式：信念是概率分布，预测用运动模型卷积，更新用量测似然加权。卡尔曼滤波是它在高斯世界里的闭式解——信念恰好能用均值与方差概括，循环变成两条轻量的公式。工程上的含义很实惠：估计是有延迟预算的，一维情形两条公式几行代码，几百维状态也只要矩阵运算。
一维卡尔曼：可运行的骨架
class Kalman1D: """一维卡尔曼滤波：估计位置，速度由调用方提供。 变量约定： x 当前估计（均值） P 估计的不确定性（方差） Q 过程噪声：运动模型自身的不可靠程度 R 量测噪声：探头读数的不可靠程度（来自标定） """ def __init__(self, x0=0.0, P0=1e3, q=1e-2, r=1.0): self.x, self.P = x0, P0 self.Q, self.R = q, r def predict(self, dt, velocity): # 预测：状态前推，不确定性按过程噪声增长 self.x += velocity * dt self.P += self.Q * dt return self.x def update(self, z): # 更新：卡尔曼增益决定信模型还是信量测 K = self.P / (self.P + self.R) self.x += K * (z - self.x) # 残差按增益回灌 self.P *= (1 - K) # 更新后方差收缩 return self.x, K # 融合两个异构探头：各建一路滤波，按精度反比加权 class FusedAltitude: def __init__(self): self.kf_baro = Kalman1D(r=4.0) # 气压计：稳但漂 self.kf_laser = Kalman1D(r=0.5) # 激光测距：准但怕雨雾 def step(self, dt, v_up, z_baro=None, z_laser=None): for kf in (self.kf_baro, self.kf_laser): kf.predict(dt, v_up) # 共用同一运动模型预测 ws = [] if z_baro is not None: ws.append((self.kf_baro.update(z_baro)[0], 1.0 / self.kf_baro.R)) if z_laser is not None: ws.append((self.kf_laser.update(z_laser)[0], 1.0 / self.kf_laser.R)) if not ws: return self.kf_baro.x, 0.0 # 全失明：退回纯预测并标低置信 total_w = sum(w for _, w in ws) est = sum(x * w for x, w in ws) / total_w conf = min(1.0, total_w / (total_w + self.kf_baro.P)) return est, conf
读这段代码要盯住两条线：增益 K 是"信谁"的旋钮——P 大（模型已不可靠）则 K 趋近于 1，量测说了算；P 小则 K 趋近 0，模型说了算。方差 P 在预测时增长、更新时收缩，正是"量测缺失时不确定性上升"的代码形态。融合层的加权用各自量测噪声的倒数——探头越准话语权越大，失明的那路自动闭嘴。这套骨架推广到多维就是标准卡尔曼滤波，非线性模型换 EKF 或 UKF 的预测更新内核，思想完全一致。
估计器的健康自检也值得一并实现——一致性检验就是它的体检仪：
class ConsistencyMonitor: """一致性检验：残差长期偏大或偏小，都是参数失配的指纹。""" def __init__(self, kf, window=200): self.kf = kf self.window = deque(maxlen=window) def feed(self, z): predicted = self.kf.x # 更新前的预测 residual = z - predicted expected = (self.kf.P + self.kf.R) ** 0.5 self.window.append(residual / expected) # 归一化残差 def verdict(self): mean = sum(self.window) / len(self.window) if mean > 1.5: return "量测残差偏大：R 偏小或模型失配" if mean < 0.4: return "残差偏小：过度自信，Q 或 R 设小了" return "一致"
多源融合的三种形态
多探头信息融合按情报关系分型，处置各不同：互补型（各看一段，拼起来才完整，如相机给语义、雷达给距离）重点在时空对齐与配准；竞争型（各报同一量、精度不同，如双高度计）重点在按噪声方差加权与一致性检验——两路读数差得离谱时，先怀疑其中一个坏了而不是平均了事；协作型（一个探头的读数用于改善另一个的工作条件，如云台把相机转向雷达发现的目标）本质是主动感知，属于规划舱与感知舱的联合决策。
定位、建图与航图模型
把估计的对象从"障碍在哪"升级到"我在哪"，就是定位问题：用里程计、IMU 与外部参照（信标、特征匹配）递推位姿信念。再进一步，边定位边给未知环境建图，就是 SLAM——本章只挂接概念接口：SLAM 输出的占据栅格与特征地图，是 2.4 节内部航图几何层的主要原料。地图模型按用途选：占据栅格适合几何避障与局部规划，拓扑地图适合大范围路由（"从 A 区到 B 区走哪条通道"），特征地图适合精确定位——三者在 2.4 节合并成分层航图，本节只需保证估计器输出带置信度的占据信息。
图：多源融合状态估计——预测与更新的双线合流
联调测试与故障排查
估计段的联调用回放对分法：把录制好的真实任务数据回放给估计器，与事后离线精算的轨迹对分，统计误差分布是否在标称噪声内。再跑一致性检验：量测残差应长期与理论方差相符——残差长期偏大说明过程噪声 Q 设小了（模型被过度信任），长期偏小说明量测噪声 R 设小了（探头被过度信任）。这套校验是估计器参数唯一的科学调法，比手感调参可靠一个量级。
按症状排查：估计发散（误差越滚越大）几乎总是 Q 与 R 失配或运动模型错了方向，先回放最小数据集复现；过度自信（置信度恒为高但实际漂移）是 P 收缩过快，检查更新频率是否被重复计费——同一量测喂了多路滤波器是常见根因；滞后拖拍（读数总慢半拍）是预测权重过大，增大量测权重或提高量测频率；雨雾天气集体漂移回 2.1 节查探头失效模式——估计器救不了集体失明的阵列。
⚠️ 常见坑：把滤波器调到"曲线好看"就收工。好看可能意味着过度平滑，延迟正在吃掉安全余量。验收看误差分布与一致性检验，不看曲线的观感。
本节要点回顾
- 量测不是状态：估计器把瞬时带噪量测织成连续带置信度的信念，仪表读数必须同时回答"是多少"与"多大把握"。
- 预测-更新循环：运动模型推信念、量测拉回偏差，增益 K 按"信模型还是信量测"动态分配，P 涨于预测、缩于更新。
- 融合分型：互补型重对齐、竞争型重加权与一致性检验、协作型是主动感知。
- 定位与建图：SLAM 输出的栅格与特征地图是 2.4 节航图几何层的原料。
- 科学调参：回放对分加一致性检验，Q、R 失配各有指纹；发散先查模型，过度自信先查重复计费。
信念已经上了仪表，但仪表读数堆在一起还不能叫"地图"。下一节看舱内航图：几何、拓扑、语义三层结构如何把这些读数组织成规划舱敢用的知识。