01.3｜从日志到模型：最小二乘、激励与残差

上一课列出了物理结构，却没有给某一台机器人填写参数。本课把“猜一个阻尼”“调一个映射”变成可重复的计算。输入是时间对齐后的命令、速度与姿态数据；输出是参数、可辨识性诊断与留出数据上的预测残差。先用单轴模型说明线性回归，再辨识项目水平效能矩阵。所有例子均为离线合成数据，练习不需要连接任何推进器。

开始前：把“拟合”变成一个可以手算的问题

先读 00.3 方程、秩与最小二乘，以及 00.2 导数与时间差分。辨识不是让程序寻找一串看起来合适的数字：先选未知参数，再写每个样本提供的方程，最后检查这些方程是否提供不同信息。先手解本课的三条无噪声方程，再读带噪声的优化推导。

符号	含义与维度	单位／坐标约定
$u_k,a_k,T_k$	前向相对水速、其导数、已标定推力	$\mathrm{m/s}$ 、 $\mathrm{m/s^2}$ 、N； $b$ 前轴
$\theta=[m_e,d_1,d_2]^T$	有效质量、线性与二次阻尼	kg、 $\mathrm{N\,s/m}$ 、 $\mathrm{N\,s^2/m^2}$
$\Phi,y,\epsilon$	$N\times3$ 回归矩阵、 $N\times1$ 力数据、残差	每行乘参数后为N
$s=[s_f,s_l]^T,G_{xy},a_{xy}^b$	2通道归一化命令、 $2\times2$ 效能、水平加速度	$s$ 无量纲； $G$ 为 $\mathrm{m/s^2}$ 每单位命令
$U,A,\Sigma,\lambda$	命令样本矩阵、加速度样本矩阵、噪声协方差、正则权重	$U$ 为 $N\times2$ ； $A$ 为 $N\times2$ ； $\Sigma$ 依输出单位

线性于参数，不要求线性于速度

暂时限制到单一平移轴，姿态近似固定、海流已知或静水、耦合项可忽略，并且推力已经由推力曲线换成牛顿。动力学对速度含有绝对值，因此是非线性运动方程；但未知参数只以一次乘法出现，所以仍是线性参数回归。每个时刻提供一行，把所有时刻纵向堆叠就得到超定方程。

\begin{aligned}T_k&=m_e a_k+d_1u_k+d_2|u_k|u_k+\epsilon_k,\\\varphi_k^T&=[a_k,\ u_k,\ |u_k|u_k],\quad y_k=T_k,\\y&=\Phi\theta+\epsilon,\quad\Phi=\begin{bmatrix}\varphi_1^T\\\vdots\\\varphi_N^T\end{bmatrix}.\end{aligned}

(1.15)

式 $(1.15)$ 先写一行数据，再谈回归

第 $k$ 行必须来自同一物理时刻或同一个积分窗口。 $T_k$ 是已经换算成牛顿的推力， $a_k$ 是机体前向速度分量导数， $u_k$ 是相对水速度；若直接使用对地DVL速度，海流就会被错误地塞进阻尼参数。参数向量 $\theta$ 的三个分量不同单位，而回归行三项乘相应参数后都应成为牛顿。

例如 $a_k=0.1\ \mathrm{m/s^2}$ 、 $u_k=0.5\ \mathrm{m/s}$ ，参数 $(30,8,12)^T$ 预测 $T_k=3+4+3=10\ \mathrm N$ 。二次阻尼写成 $|u|u$ 而不是 $u^2$ ，因为反向运动时阻力也要反向。令 $u=-0.5$ 且 $a=0$ ，预测推力为 $-7\ \mathrm N$ 。这个正反号测试应在拟合真实数据之前通过。

平方误差是一个二次函数。对参数求导，先用链式法则产生负回归矩阵转置，再令梯度为零，就得到正规方程。满列秩使解唯一；若列相关，存在多组参数产生同样的预测。正规方程用于手推，实际求解优先QR或SVD，因为显式构造平方矩阵会把条件数平方。TB01-S4

\begin{aligned}J(\theta)&=\tfrac12(y-\Phi\theta)^T(y-\Phi\theta),\\\nabla_\theta J&=-\Phi^T(y-\Phi\theta)=0,\\\Phi^T\Phi\hat\theta&=\Phi^Ty,\qquad\hat\theta=(\Phi^T\Phi)^{-1}\Phi^Ty\quad(\operatorname{rank}\Phi=3).\end{aligned}

(1.16)

不跳过求导：先对每一个未知参数展开

设 $\Phi_{ki}$ 是第 $k$ 个样本、第 $i$ 个参数的已知系数， $\theta_i$ 是未知参数， $y_k$ 是该样本观测。残差与目标函数为

r_k=y_k-\sum_{i=1}^{3}\Phi_{ki}\theta_i, \qquad J=\frac12\sum_{k=1}^{N}r_k^2.

$N$ 是样本数。只改变第 $j$ 个参数时， $\partial r_k/\partial\theta_j=-\Phi_{kj}$ ，因为其他参数暂时固定。平方求导产生二，恰好抵消二分之一：

\frac{\partial J}{\partial\theta_j} =\sum_{k=1}^{N}r_k(-\Phi_{kj}).

把 $j=1,2,3$ 三个导数竖着堆起来，才得到 $-\Phi^Tr$ 。梯度为零意味着往任何参数方向小改一点都不能一阶降低代价；还要检查 $\Phi^T\Phi$ 正定，才能保证唯一最小值。满列秩时，对任意非零变化 $\delta\theta$ 都有 $\delta\theta^T\Phi^T\Phi\delta\theta=\|\Phi\delta\theta\|^2>0$ ，这正是所需条件。样本数大于参数数不保证满秩，重复同一工况仍可能缺信息。

式 $(1.16)$ 从几何上说，是让残差垂直于所有解释变量

$J$ 的微分为 $dJ=-(y-\Phi\theta)^T\Phi\,d\theta$ ；将 $d\theta$ 前的系数转置，就得到梯度。最优时 $\Phi^Tr=0$ ，其中 $r=y-\Phi\hat\theta$ 是残差。因此残差与回归矩阵每一列正交，意味着在当前模型允许的方向上继续调整参数不能再降低误差。它不意味着每个残差为零，更不意味着遗漏的物理项不存在。

正规矩阵的逆写法只是闭式表达。手写实现可用带列尺度的QR，或者在本课三参数的小例中使用带主元消元来观察过程。若主元接近零，应报告不可辨识，而不是用极小数替代主元继续生成巨大参数。训练段残差小而验证段残差大时，首先查看工况是否变化以及噪声是否进入回归量。

手算三行辨识：第一行取速度零、加速度1 $\mathrm{m/s^2}$ 、推力30 N，立即得到有效质量30 kg。第二行取速度1 $\mathrm{m/s}$ 、加速度零、推力20 N，得到 $d_1+d_2=20$ ；第三行速度2 $\mathrm{m/s}$ 、加速度零、推力64 N，得到 $2d_1+4d_2=64$ 。第三式减两倍第二式给 $2d_2=24$ ，故 $d_2=12$ 、 $d_1=8$ 。这里两种速度幅值使线性与二次阻尼可以区分；只有一个速度幅值时，无论记录多久都无法完成同样任务。

\begin{bmatrix}1&0&0\\0&1&1\\0&2&4\end{bmatrix}\begin{bmatrix}m_e\\d_1\\d_2\end{bmatrix}=\begin{bmatrix}30\\20\\64\end{bmatrix}\quad\Longrightarrow\quad\hat\theta=\begin{bmatrix}30\\8\\12\end{bmatrix}.

(1.17)

式 $(1.17)$ 是刻意设计的三次实验

第一行隔离质量，后两行隔离阻尼，设计目的不是让数据“好看”，而是让未知数可区分。加速度一的单位为米每二次方秒，所以第一行的三十不是无量纲比例，而是有效质量三十千克。第二、第三行都是稳态点，它们不能单独告诉你质量。

如果将第三行速度二改成速度一，后两行完全相同，方程组不再唯一。即使每行重复采集一百万次，仍然不能修复这个结构问题。实际实验会使用更多有噪声的数据，最小二乘是把这种可区分的关系平均起来；实验设计和估计器不能互相替代。

为什么有些实验“数据很多，信息很少”

若全程匀速，第一列加速度全为零，质量根本未进入观测；若始终以同一速度绝对值运动，后两列成比例，线性和二次阻尼混在一起。激励应覆盖加速、减速、多个速度幅值及正反方向，还应避免执行器一直饱和。用奇异值描述这种信息：最小奇异值接近零意味着某个参数组合几乎不影响输出。比较条件数前，应先按有物理意义的尺度归一化列，否则单位差异会伪装成坏条件数。

\begin{aligned}\Phi&=U_\Phi\operatorname{diag}(\sigma_1,\ldots,\sigma_p)V_\Phi^T,\\\operatorname{rank}(\Phi)&=p\ \Longleftrightarrow\ \sigma_p>0,\qquad\kappa_2(\Phi)=\sigma_1/\sigma_p,\\\operatorname{Cov}(\hat\theta)&=\sigma_\epsilon^2(\Phi^T\Phi)^{-1}\quad\text{独立同方差、回归量无误差时}.\end{aligned}

(1.18)

式 $(1.18)$ 中的奇异值在量什么

$U_\Phi$ 和 $V_\Phi$ 是左、右奇异向量矩阵，不是前面的控制命令矩阵； $p$ 在这里是参数个数，不是位置或角速度。沿某个右奇异向量改变参数一个单位，输出变化大小就是对应奇异值。最小奇异值很小意味着这一组合只能从微弱的输出变化中辨认，因此特别容易受噪声影响。

协方差式还用了回归量无误差与独立同方差残差的假设。若速度含噪声且又被用来构造回归矩阵，这些条件就不满足，公式可能过于乐观。实际判断时同时报告尺度归一化后的奇异值、验证误差和参数物理合理性，不应把很小的训练误差当成参数可靠性的全部证据。

加速度通常不是直接测得的机体分量导数：IMU比力要补重力、偏置和旋转项；速度差分会放大噪声。若速度噪声独立且方差为 $\sigma_v^2$ ，前向差分的噪声方差是 $2\sigma_v^2/\Delta t^2$ 。更高采样率不自动让差分加速度更干净。可先时间对齐并采用平滑微分，也可把方程在窗口积分，减少直接微分；积分后各行噪声往往相关，需要重新考虑权重。

\begin{aligned}\hat a_k&=\frac{u_{k+1}-u_k}{\Delta t},\quad\operatorname{Var}(\hat a_k)=\frac{2\sigma_v^2}{\Delta t^2},\\\int_{t_0}^{t_1}T\,dt&=m_e[u(t_1)-u(t_0)]+d_1\int_{t_0}^{t_1}u\,dt+d_2\int_{t_0}^{t_1}|u|u\,dt.\end{aligned}

(1.19)

式 $(1.19)$ 说明为何“采样更快”不等于“差分更准”

两帧独立速度噪声相减，方差相加成为 $2\sigma_v^2$ ；除以时间间隔后，方差再除以 $\Delta t^2$ 。如果速度噪声标准差是 $0.01\ \mathrm{m/s}$ ，间隔 $0.1\ \mathrm s$ 时差分标准差约 $0.1414\ \mathrm{m/s^2}$ ，间隔缩到 $0.01\ \mathrm s$ 则变为 $1.414\ \mathrm{m/s^2}$ 。这不是物理加速度变大，而是微分放大高频误差。

积分式则直接用牛顿方程在窗口内积分。质量项通过微积分基本定理变成端点速度差，阻尼项保留数值积分。实现可用梯形积分，但两端速度误差仍存在，重叠窗口也会引入相关噪声。选择窗口长度是在时间分辨率与噪声之间折中，不是任意越长越好。

若推力 $T$ 本身未知，只知道归一化命令 $s$ 并假设 $T=ks$ ，那么将整个运动方程乘同一个非零比例不会改变预测。没有独立质量或推力标定，能识别的通常是 $k/m$ 、 $d_1/m$ 、 $d_2/m$ ，而不是四个绝对物理量。先判断参数是否在数学上可区分，比选择更复杂的优化器更重要。闭环数据还有一个问题：控制命令可能与测量噪声相关，普通最小二乘的无偏条件不再自动成立；应保留独立验证段，必要时用独立激励或工具变量。

项目的水平效能矩阵：两列输入决定四个系数

在一个姿态、速度和负载附近，可把机体水平加速度写成两个归一化通道命令的线性映射。 $G$ 的每列就是只激励一个通道时的两个轴响应。它把推进器、混控、惯性与局部工作点综合在一起，是经验效能，不是六自由度惯性 $M$ ，也不是几何分配 $B$ 。对N条记录，用每行存一个命令，矩阵维度决定了 $G$ 转置的位置。

\begin{aligned}a_{xy}^b&=G_{xy}s+b_a,\quad G_{xy}=\begin{bmatrix}5.33&-0.83\\1.39&3.21\end{bmatrix},\\A&=UG_{xy}^T+\mathbf1 b_a^T+E,\quad\widetilde U=[U\ \mathbf1],\\\begin{bmatrix}\widehat G_{xy}^T\\\hat b_a^T\end{bmatrix}&=\operatorname*{argmin}_{\Theta}\|A-\widetilde U\Theta\|_F^2.\end{aligned}

(1.20)

式 $(1.20)$ 怎样逐列恢复水平效能

$s_f$ 和 $s_l$ 是无量纲前向、侧向命令， $b_a$ 是两维加速度偏置。四个正负轴向样本足以给出直观解：前向正负响应之差除以命令差，得到第一列；侧向正负响应之差得到第二列；对称响应均值给偏置。增广矩阵最后一列全一，使两轴偏置一起作为未知数估计。

例如前向命令 $0.2$ 与 $-0.2$ 的前向加速度差为 $2.132\ \mathrm{m/s^2}$ ，相除得 $5.33$ ；同样两次实验的侧向加速度差为 $0.556\ \mathrm{m/s^2}$ ，得到交叉项 $1.39$ 。先识别列的物理含义，再写矩阵转置，可避免把“前推引起侧移”与“侧推引起前移”交换。

偏置列吸收小的常值扰动力或传感器零偏；它不应替代明确的重力补偿。若两个命令永远相等， $U$ 只有一维信息，只能看见 $G$ 两列之和。训练前应先核对实际施加命令，而非饱和前请求。对于项目矩阵，行列式是18.2630，逆映射将目标 $(1.0,0.5)\,\mathrm{m/s^2}$ 变成约 $(0.19848875,0.06981328)$ ；代回原矩阵应恢复原目标。

G_{xy}^{-1}=\frac1{18.263}\begin{bmatrix}3.21&0.83\\-1.39&5.33\end{bmatrix},\qquad s=G_{xy}^{-1}\begin{bmatrix}1\\0.5\end{bmatrix}.

(1.21)

式 $(1.21)$ 的逆映射不包含阻尼预测

二阶矩阵求逆来自余子式：对角元素交换、非对角元素反号，再除以行列式。行列式非零表示两个命令在当前线性近似下产生独立的加速度方向。它不保证命令在允许范围内，也不保证离开辨识工作点后仍准确。

本例分子分别是 $3.21+0.83\times0.5=3.625$ 与 $-1.39+5.33\times0.5=1.275$ ，再除以 $18.263$ 得到两条命令。若目标加速度整体放大十倍，命令也放大十倍，很快会超出归一化边界。将超界命令公共缩放能保持方向，但会牺牲幅值；还需要将可实现加速度反馈给速度积分器。

加权、正则与残差应回答不同问题

噪声方差不同的样本可使用逆协方差加权；正则项则体现参数向先验靠拢，两者不是同一种“平滑”。对下面目标求导仍得到线性方程。 $L$ 是参数尺度或差分结构， $\theta$ 0是先验；若用单位阵，必须先把不同单位的参数缩放到可比较尺度。正则能稳定数值，但不能凭空创造未激励方向的信息。

\begin{aligned}J&=\tfrac12(y-\Phi\theta)^T\Sigma^{-1}(y-\Phi\theta)+\tfrac\lambda2\|L(\theta-\theta_0)\|^2,\\(\Phi^T\Sigma^{-1}\Phi+\lambda L^TL)\hat\theta&=\Phi^T\Sigma^{-1}y+\lambda L^TL\theta_0.\end{aligned}

(1.22)

式 $(1.22)$ 把可信程度和先验约束分开

$\Sigma$ 是观测噪声协方差，不是参数协方差。若某条力测量标准差是其他测量的两倍，其方差四倍，相应逆方差权重只有四分之一。 $\lambda$ 控制先验项强度， $L$ 给参数尺度或结构， $\theta_0$ 是先验中心。数据项和先验项各求一次梯度，移项后才得到右端的先验贡献。

当某个方向没有激励时，正则化仍可输出一个数，但该方向主要来自先验。应通过改变先验强度、留出工况与残差结构来理解这个数，而不是把它称为实验发现。程序可先对数据做白化，再把先验伪观测拼成增广最小二乘，避免直接构造一个条件很差的大正规矩阵。

拟合之后看残差，而不是只看参数的小数位。残差均值明显不为零，可能缺偏置；随速度增长，可能缺二次阻尼；与延迟后的命令相关，可能遗漏执行器动态或时间偏移。白残差是支持模型的一条证据，不是物理正确的充分条件。按整段时间划分训练与验证，避免把强相关的相邻样本随机散到两边，制造虚假的泛化表现。

参考实现：C++17，先独立手写再展开

// 独立教学实现：修正Gram-Schmidt QR求最小二乘，C++17
#include <array>
#include <vector>
#include <cmath>
#include <cassert>
#include <stdexcept>
#include <iostream>
using Row=std::array<double,3>;
Row leastSquares(std::vector<Row> X,const std::vector<double>& y){
    const int n=static_cast<int>(X.size());
    if(n<3 || y.size()!=X.size())throw std::runtime_error("bad shape");
    double R[3][3]{}; std::vector<Row> Q(n);
    for(int j=0;j<3;++j){
        std::vector<double> z(n);for(int k=0;k<n;++k)z[k]=X[k][j];
        for(int i=0;i<j;++i){
            for(int k=0;k<n;++k)R[i][j]+=Q[k][i]*z[k];
            for(int k=0;k<n;++k)z[k]-=R[i][j]*Q[k][i];
        }
        for(double x:z)R[j][j]+=x*x;
        R[j][j]=std::sqrt(R[j][j]);
        if(R[j][j]<1e-10)throw std::runtime_error("rank deficient");
        for(int k=0;k<n;++k)Q[k][j]=z[k]/R[j][j];
    }
    Row rhs{},theta{};
    for(int j=0;j<3;++j)for(int k=0;k<n;++k)rhs[j]+=Q[k][j]*y[k];
    for(int j=2;j>=0;--j){
        double value=rhs[j];for(int k=j+1;k<3;++k)value-=R[j][k]*theta[k];
        theta[j]=value/R[j][j];
    }
    return theta;
}
int main(){
    auto theta=leastSquares({{1,0,0},{0,1,1},{0,2,4}}, {30,20,64});
    assert(std::abs(theta[0]-30)<1e-10);
    assert(std::abs(theta[1]-8)<1e-10 && std::abs(theta[2]-12)<1e-10);
    std::vector<Row> X{{.2,0,1},{-.2,0,1},{0,.2,1},{0,-.2,1}};
    std::vector<double> ax,ay;
    for(auto x:X){ax.push_back(5.33*x[0]-.83*x[1]+.03);ay.push_back(1.39*x[0]+3.21*x[1]-.02);}
    auto gx=leastSquares(X,ax),gy=leastSquares(X,ay);
    assert(std::abs(gx[0]-5.33)<1e-10 && std::abs(gy[1]-3.21)<1e-10);
    bool rejected=false;
    try { leastSquares({{1,1,1},{2,2,1},{3,3,1}},{1,2,3}); }
    catch(const std::runtime_error&){rejected=true;}
    assert(rejected);
    std::cout<<theta[0]<<" "<<theta[1]<<" "<<theta[2]<<"\n";
}

项目映射：libraries/AC_PosControl 中 update_xy_controller_poshold_impl 生成加速度目标，accel_target_to_xy_thrust 完成姿态转换与效能逆映射。先将 $\mathrm{cm/s^2}$ 除100成为 $\mathrm{m/s^2}$ ，再对 $G$ 求解；AP_Motors6DOF后续还会限幅，因此辨识数据应记录真正应用的命令。DVLVelocityFilter::update 与惯导输出的时间关系，也决定了命令—加速度配对是否成立。

手写练习：不调用优化库，先用三行数据手解质量和阻尼；然后用四条正负轴向命令求 $G$ 与偏置。答案要点：对每轴正负响应取差除以0.4得到相应 $G$ 列，正负响应取平均得到偏置。把所有命令改成（t,t），再观察最小奇异值和矩阵秩；答案是无法分别恢复两列。最后在验证数据加入0.1 $s$ 命令延迟，画残差与命令互相关，应出现非零时移结构，而不是简单宣称“噪声增大”。

TB01-S4 · NumPy 开发者：numpy.linalg.lstsq

精读返回的rank与singular values；残差数组为空不等于误差必然为零，应自行计算 $A$ -X $\Theta$ 。

从正规方程走到可以手写的QR求解器

为什么参考实现没有直接计算 $(\Phi^T\Phi)^{-1}$ ？因为两个几乎相关的回归方向在正规矩阵里会变得更难区分。QR方法先把回归列分解为正交方向与上三角系数。令 $\Phi=Q_RR_R$ ，其中 $Q_R^TQ_R=I$ ， $R_R$ 是上三角矩阵；下标只是避免与滤波噪声矩阵重名。把残差分成列空间投影和正交补，两部分平方相加，得到：

\|y-\Phi\theta\|^2 =\|Q_R^Ty-R_R\theta\|^2 +\|(I-Q_RQ_R^T)y\|^2.

第二项不含未知参数，因此只需令第一项最小。满列秩时上三角矩阵可逆，从最后一个参数开始回代，便得到 $R_R\hat\theta=Q_R^Ty$ 。程序中 rhs 对应右端，逆序循环对应回代；没有任何一步需要显式求逆。若某个对角主元很小，意味着一个回归列几乎落在前面列的张成空间中，应停止并重新设计激励。

教学代码用修正Gram–Schmidt：每次从当前待处理列中减去已知正交列的投影，再归一化。对于差异很大的单位，先按预计参数尺度归一化回归列；高条件数生产问题更适合Householder QR或SVD。此处目的不是重写工业线性代数库，而是让每一次点乘、投影和回代都能与最小二乘推导对应。

你可以继续完成一个不依赖外部库的完整实验：生成前向正负加速与多个稳态速度的数据；用已知参数算出无噪声推力；加入固定随机种子的测量噪声；将前半段用于拟合、后半段用于预测；分别输出每个参数与每条残差。然后删去所有加速段，再运行相同求解器。正确行为不是输出一串看似精确的质量，而是指出质量方向没有信息。将两种实验放在一起，才能真正理解算法和数据各自承担的责任。

零基础停步自检

三个参数记录了一千个完全相同的稳态速度样本，是否一定能辨识？

展开答案与原因

不能。稳态加速度列为零，且相同速度重复不会增加矩阵秩；质量不可辨识，两个阻尼贡献也无法分开。

01.3｜从日志到模型：最小二乘、激励与残差

开始前：把“拟合”变成一个可以手算的问题

线性于参数，不要求线性于速度

式(1.15)(1.15)(1.15)先写一行数据，再谈回归

不跳过求导：先对每一个未知参数展开

式(1.16)(1.16)(1.16)从几何上说，是让残差垂直于所有解释变量

式(1.17)(1.17)(1.17)是刻意设计的三次实验

为什么有些实验“数据很多，信息很少”

式(1.18)(1.18)(1.18)中的奇异值在量什么

式(1.19)(1.19)(1.19)说明为何“采样更快”不等于“差分更准”