03.3｜从模型残差到扰动观测器：估计未知作用，而不是盲目加积分

设模型能预测控制输入造成的加速度，真实速度却持续偏离预测。差异可能来自海流、阻尼失配、推进器效率或输入实现误差。扰动观测器把这些作用汇集成一个可估计量，并用反向输入补偿。输入是同步的速度与实际控制加速度，输出是扰动估计及补偿量。本课首先假定扰动在观测器时间尺度上近似常数。

需要补的数学只读这些： 导数与离散化和秩与状态空间。先能口头解释本页小例，再进入完整推导；不必预先背下矩阵公式。

观察不到的力，为什么能从速度中推断

若模型预计艇会减速，真实速度却保持不变，必有模型尚未解释的作用补上了缺口。扰动观测器估计的就是这个缺口，而不是凭空安装了一只测海流的传感器。它可以包含海流作用、阻尼误差和执行器误差，因此叫“集总扰动”；把估计曲线直接命名为海流速度通常不成立。

先区分三种角色。 控制器根据目标决定输入；观测器根据已知输入、测量与模型重建未知状态；低通滤波器可以仅平滑一个读数。卡尔曼滤波器也是状态观测器的一种，只是用协方差决定修正权重。本课用极点选择观测增益，要求理想误差衰减；它没有因此自动获得概率置信区间。

创新注入为什么取这个方向

若读数 $y=0.35\,\mathrm{m/s}$、预测 $\hat v=0.30\,\mathrm{m/s}$，创新为正。增加速度估计是立即承认“先前低估”；增加扰动估计则尝试解释“为什么未来还可能继续低估”。前者修正当前值，后者修正未来预测的原因。只有在模型与可观测性支持时，这个解释才可靠；单帧噪声也能造成同样正创新，所以增益不能无限大。

微练习： 将观测带宽翻倍，是否必然让补偿更好？如果实际输入限幅而观测器仍使用未限幅命令，会发生什么？

残差公式为什么正确，直接实现又为什么危险

残差是一个可计算的平衡式。 测量 $y$、噪声 $n_v$ 都为速度；$s y$ 和 $ay$ 都为加速度，才能与输入 $u$ 相减。低通 $Q_f$ 的分子分母都有频率单位，整个传递函数无量纲。手算无噪声瞬间：$a=1\,\mathrm{s^{-1}}$、$v=0.3\,\mathrm{m/s}$、$u=0.1\,\mathrm{m/s^2}$、真实 $\dot v=0$，则残差为 $0+0.3-0.1=0.2\,\mathrm{m/s^2}$，即持续外力恰好补上阻尼不足的部分。这个代数值不是经过低通后的瞬时估计值，滤波状态还需要时间收敛。

第二行只是移项：观测到的总加速度，扣除模型解释的阻尼与控制作用，剩下的就是集总扰动。第三行对残差低通。这里 $s$ 是拉普拉斯变量，推导传递函数时取零初始条件； $Q_f$ 无量纲，不是卡尔曼过程噪声协方差。微分会放大高频测量噪声，一阶 $Q_f$ 仅使高频增益有界，不让它降到零；需要进一步低通、降低带宽或采用状态观测实现。

若推进器请求为 1 $\mathrm{m/s^2}$，却因饱和只实现 0.6，仍把 1 填入残差，就会多估计 −0.4 $\mathrm{m/s^2}$ 的“扰动”。控制器随后试图抵消自己的限幅，可能和积分器争抢。因此至少使用限幅后的已知输入，并把未知推进器动力学视为模型误差，而不是凭空认为实际输入等于命令。

不用数值微分的两状态观测器

矩阵每个位置都有含义。 $A_o$ 第一行 $(-a,1)$ 表示速度受阻尼与扰动共同影响；第二行全零表示本地常扰动假设。$B_o$ 第一项为一表示输入直接作用在速度导数，第二项为零表示控制命令不直接改变外部扰动本身。$C_o=(1,0)$ 表示传感器只读速度。把这些矩阵乘开就能检查状态排列是否弄反。$Lr$ 第一项必须为加速度，第二项必须为 $\mathrm{m/s^3}$，故两项增益不能共用同一个数值与单位。

第二个状态的导数设为零，表示短时常扰动假设，不是说海流永远不变。输出只能直接测速度，但扰动会改变下一时刻速度，所以可以通过时间演化间接辨识。可观测性矩阵的第一行是 $C_o$，第二行是 $C_oA_o$；行列式为一，两个状态均可观测。注意不同状态具有不同单位，矩阵里的数字一实际上承担相应单位映射。

可观测性在这里意味着两条信息独立。 第一行给出瞬时速度；沿已知动力学对输出求导，再扣除已知输入，得到 $\dot y-u=-av+\delta$。这就是第二行 $C_oA_o=(-a,1)$，包含第一行没有的扰动信息。两行不能互相倍乘，矩阵满秩为二，才可能仅凭一条速度时间序列恢复两个状态。该判据建立在输入已知、模型已知的前提上；它不是说任意短、任意噪声大的数据都能精确分离两者。实际可观测性强弱还取决于采样、噪声与时间窗口。

创新统一用测量减预测。实际速度大于预测时创新为正，第二式增加正扰动估计，下一次预测便得到额外正加速度，方向自洽。令估计误差为真实减估计，先忽略测量噪声与扰动变化，误差动力学是 $A_o-LC_o$，而非 $A_o+LC_o$。对这个二阶矩阵直接求特征行列式即可配极点。

行列式展开一步都不要跳。 $sI-A_o+LC_o$ 的四项为 $(s+a+l_1,-1;l_2,s)$，其行列式是 $(s+a+l_1)s-(-1)l_2$，因此常数项是正的 $l_2$。再把期望多项式 $s^2+2\omega_os+\omega_o^2$ 与它按次数比较，才得到两个增益。若创新误写为预测减测量，却不同时改变增益注入符号，就会变成 $A_o+LC_o$，这个配极点结果也不再成立。对于 $a=1,\omega_o=4$，矩阵为 $(-8,1;-16,0)$，其迹为 $-8$、行列式为 $16$，与两个 $-4$ 的和及积一致。

例如 $a=1\,\mathrm{s^{-1}}$、希望两个观测极点都为 −4 $\mathrm{s^{-1}}$，则 $l_1=7\,\mathrm{s^{-1}}$、$l_2=16\,\mathrm{s^{-2}}$。创新 0.05 m/s 给速度估计额外 0.35 $\mathrm{m/s^2}$，给扰动估计变化率 0.8 $\mathrm{m/s^3}$。若周期 0.01 s，扰动估计仅增加 0.008 $\mathrm{m/s^2}$，不是直接增加 0.8。欧拉离散极点为 $1-4T=0.96$；这是数值实现检查，不是连续极点稳定就自动保证任意采样稳定。

先区分估计误差与补偿输入。 定义 $\tilde v=v-\hat v$、$\tilde\delta=\delta-\hat\delta$。真实扰动增加而估计尚未跟上时，第二个误差方程多出 $\dot\delta$；观测器受到 $+Ln_v$ 注入，故真实减估计误差中是 $-Ln_v$。这解释了最后一行两个附加项的符号。控制补偿只有在扰动与输入作用于同一动力学通道时才能直接用 $-\hat\delta$ 消去；例如横向未知作用不能直接用只具有前向效能的执行器抵消，必须经过可达域和分配分析。

补偿用负号，理想时真实扰动与估计相消。最后一行保留了前面略去的两项，直接展示带宽代价：较大增益能更快跟踪缓慢变化的扰动，却也更强地注入噪声。若模型含输入延迟、非最小相位零点或未建模执行器动态，不能无限增大观测带宽。鲁棒控制的核心在于对这些不确定性保留稳定裕度，而不是把估计曲线调得最平滑或最快。

DOB、增广 EKF、ADRC：相似的状态，不同的设计问题

DOB 是一类利用模型与输入输出残差估计扰动的结构，可和原有 PID 组合；本课是线性状态空间实现。增广 KF/EKF 也可以把扰动加入状态，给它随机游走过程噪声，但增益由模型、协方差与观测更新共同决定；这里的 $L$ 是按确定性极点指定。ADRC 常把未知动态合并为“总扰动”，用扩张状态观测器及补偿构成控制框架；对本课一阶对象，ESO 的状态形式可以相近，但还须说明名义输入增益和控制律，不能把任意加了一个状态的滤波器统称 ADRC。TB03-S3

速度传感器偏置与真实外扰也不能自动区分：若观测只有带未知常偏的速度，而实际速度未被独立约束，增广更多状态可能失去可辨识性。先写测量方程、检验可观测性，再谈“同时估计海流、偏置与阻力”。积分器与 DOB 都能消除某些慢偏差，叠加时要分清带宽与职责，避免两者在饱和时同时累计。

// 教学实现：欧拉离散两状态DOB与速度P，单位全部SI。
#include <algorithm>
#include <cassert>
#include <cmath>
#include <iomanip>
#include <iostream>
struct Observer {
    double vh=0,dh=0;
    void step(double y,double applied,double dt) {
        const double a=1,l1=7,l2=16;
        const double innovation=y-vh;
        const double dvh=-a*vh+applied+dh+l1*innovation;
        const double ddh=l2*innovation;
        // 两个右端使用同一份旧状态，不能先改dh再算dvh。
        vh+=dt*dvh; dh+=dt*ddh;
    }
};
int main(){
    Observer o; double v=0;
    const double dt=.01,ref=.3;
    for(int k=0;k<2000;++k){
        const double disturbance=k>=200 ? .2 : 0;
        const double y=v;
        const double baseline=ref+2*(ref-y);
        const double applied=std::clamp(baseline-o.dh,-1.0,1.0);
        o.step(y,applied,dt);
        v+=dt*(-v+applied+disturbance);
    }
    std::cout<<std::setprecision(12)<<v<<' '<<o.dh<<'\n';
    assert(std::abs(v-ref)<1e-5 && std::abs(o.dh-.2)<1e-5);
}

项目阅读映射：DVL/EKF 输出的速度是该实验的测量入口，AC_PosControl_Sub 生成的加速度需求与 accel_target_to_xy_thrust 后的可达加速度是两个不同候选输入。接入教学 DOB 时应先做离线日志回放，在同一坐标、同一时间对齐速度与输入；它是本章的扩展实验，不是把 NavEKF3 的速度融合重命名成扰动观测器。现有 Z 控制、水平积分与最终 mixer 限幅都要纳入补偿边界。

手写练习：把观测带宽从 4 改为 8，a 不变，求增益与欧拉极点。

TB03-S3 · W.-H. Chen, J. Yang, L. Guo, S. Li (2016) — Disturbance-Observer-Based Control and Related Methods—An Overview

IEEE Transactions on Industrial Electronics 63(2), 1083–1095；按估计机制、补偿机制与方法联系阅读。

从连续观测器到实际任务周期：一条不能省略的时间轴

速度测量来自时刻 $t_k$，控制命令是在区间 $[t_k,t_{k+1})$ 作用。教学显式欧拉按“用 $y_k$ 与旧观测状态算创新 → 生成并限幅控制 → 用同一份旧状态计算观测导数 → 同步推进观测与对象”执行。若 DVL 传来的其实是区间平均速度，必须先明确该平均量代表的时刻；若直接把延迟测量和当前预测相减，观测器会把正常加减速误认为外扰。这里的输入对齐与前面 NavEKF 的延迟融合虽然在不同层，基本问题都是比较同一个时刻的量。

离散矩阵可以直接写成

$I_2$ 是二维单位矩阵，不是 PID 积分状态；$T$ 为采样秒数。这个公式来自把 $\dot{\hat x}$ 近似成相邻状态差除以 $T$，然后把旧状态项收集到一起。由于创新 $y-C\hat x$ 已被展开，不能在右边又额外加一遍 $TLr$，否则会重复修正。两重连续极点 $-\omega_o$ 对应欧拉极点 $1-T\omega_o$，故该离散观测误差系统需要 $0<T\omega_o<2$。例如 $\omega_o=4\,\mathrm{s^{-1}}$，若错误地用 $T=0.6\,\mathrm s$，离散极点为 $-1.4$，即使连续设计稳定，程序也会发散。

本课配极点是重复极点，误差响应一般还含 $t e^{-\omega_ot}$，不只有单个指数。因此把 $1/\omega_o$ 直接当作精确“稳定时间”是不严谨的。用仿真报告到达某个误差带的时间时，应写清初值、扰动形状和允许误差，而不是只报带宽。噪声实验则可使用确定性正弦输入，保证每次调参比较的是同一个数据序列，而不是被不同随机样本误导。

一个可手算的采样周期

设旧估计为 $\hat v_k=0.20\,\mathrm{m/s}$、$\hat\delta_k=0.10\,\mathrm{m/s^2}$，测量 $y_k=0.25\,\mathrm{m/s}$，已知执行输入 $u_k=0.05\,\mathrm{m/s^2}$，模型 $a=1\,\mathrm{s^{-1}}$，增益为 $l_1=7\,\mathrm{s^{-1}}$、$l_2=16\,\mathrm{s^{-2}}$。创新为 $0.05\,\mathrm{m/s}$；速度估计导数为 $-0.20+0.05+0.10+7\times0.05=0.30\,\mathrm{m/s^2}$；扰动估计导数为 $16\times0.05=0.80\,\mathrm{m/s^3}$。周期 $T=0.01\,\mathrm s$ 后，新估计为 $(0.203,0.108)$，各自单位仍分别为速度和加速度。先写这条手算断言，再写两千步仿真；长曲线看起来正常不代表每拍公式正确。

增广 EKF 的同与不同，应落到递推而不是名称

若将扰动看成随机游走，可以写 $\delta_{k+1}=\delta_k+w_{\delta,k}$，其中 $w_{\delta,k}$ 是每拍加速度增量噪声。过程噪声协方差增加的是对扰动变化的容许度，不是某个海流实测方差。扩展 KF 使用预测协方差、测量方差求每拍增益；当模型本身是这里的线性模型时，实际就是普通增广 KF，不需要“扩展”二字。非线性阻尼、姿态投影等进入模型后才需要相应线性化。

两者可做受控对照：保持相同模型、相同测量、相同输入与时间戳，分别使用固定配极点增益和协方差生成的增益；比较阶跃扰动跟踪、噪声传递与缺测期间的估计行为。不能把两个估计器用了不同滤波速度、不同延迟或不同输入限幅，却把性能差异全部归因于算法名字。无论固定增益还是 KF，补偿送往哪里、实际输入是否已知、传感器偏置是否可分离，都仍须单独回答。