00.3｜从解两个未知数开始：矩阵、秩与状态空间

两个未知数，需要什么样的两个方程？

假设两台同向推进器的推力分别为 $f_1,f_2$，单位 N。合力为十牛顿，已知第一台比第二台多两牛顿：

$$f_1+f_2=10,\qquad f_1-f_2=2.$$

两式相加得到 $2f_1=12$，所以 $f_1=6$、$f_2=4$。写成矩阵只是把系数整齐存起来：

$$Af=y,\quad A=\begin{bmatrix}1&1\\1&-1\end{bmatrix},\quad f=\begin{bmatrix}f_1\\f_2\end{bmatrix},\quad y=\begin{bmatrix}10\\2\end{bmatrix}\mathrm N.$$

$A$ 是已知系数矩阵，$f$ 是未知列向量，$y$ 是已知结果。第二条若改成 $2f_1+2f_2=20$，就只是第一条重复两遍；无法知道每台分别出了多少力。因此“方程数量不少于未知数”只是必要的数量检查，独立信息才决定能否唯一解。

秩与逆：什么时候能把结果倒推回输入？

矩阵的秩是独立列（也等于独立行）的数量。刚才原矩阵秩为二；重复信息矩阵的秩为一。一个有 $n$ 个未知数的线性方程组，若存在解且列秩为 $n$，解才唯一。方程多但互相矛盾时，可能根本没有精确解。

单位矩阵 $I$ 的对角线为一，其他元素为零，满足 $If=f$。方阵的逆 $A^{-1}$ 是能撤销 $A$ 的变换的矩阵：$A^{-1}A=AA^{-1}=I$。本例

$$A^{-1}=\frac12\begin{bmatrix}1&1\\1&-1\end{bmatrix}, \qquad f=A^{-1}y=\begin{bmatrix}6\\4\end{bmatrix}\mathrm N.$$

逆不是把每个元素取倒数。非方阵没有这种双侧逆，奇异方阵也没有；“求解线性方程”却不一定需要显式形成逆。后续 C++ 实现会优先采用消元、QR 或 Cholesky 等适用分解。

只有合力方程 $f_1+f_2=10$ 时，有无限多组解。$[1,-1]^T$ 方向的变化不会改变合力，因为 $[1,1][1,-1]^T=0$；这叫零空间方向。推进器冗余就是利用这种不改变需求的自由度，不是凭空获得更多可控轴。

数据有噪声：从“必须都满足”改为“总误差尽量小”

两次测得同一恒力分别是 $y_1=2\ \mathrm N$、$y_2=4\ \mathrm N$。未知力 $\theta$ 不可能同时等于二和四。定义残差 $r_i=y_i-\theta$，让平方误差和最小：

$$J(\theta)=\frac12[(2-\theta)^2+(4-\theta)^2].$$

$J$ 的单位在这里是 $\mathrm{N^2}$，前面的二分之一只为求导简洁。按普通多项式展开并求导：

$$J=\theta^2-6\theta+10, \qquad \frac{dJ}{d\theta}=2\theta-6, \qquad \hat\theta=3\ \mathrm N.$$

帽子表示估计值。二阶导数为正二，所以驻点是最小值，而不是最大值。最优残差为 $[-1,1]^T\ \mathrm N$，不是零。把多个未知数、多个样本合起来，得到第一章将逐项推导的最小二乘 $\min_\theta\frac12\|y-\Phi\theta\|^2$；$\Phi$ 是已知回归矩阵。秩不足时，优化也不能凭空创造缺失的信息。

状态空间：把记忆系统未来所需的量装进一个列向量

一维物体的位置为 $p$、速度为 $v$，输入加速度为 $u$。只知道当前位置，不知道速度，就无法预测下一刻；因此取状态 $x=[p,v]^T$。连续模型是

$$\dot x=Ax+Bu, \quad A=\begin{bmatrix}0&1\\0&0\end{bmatrix},\quad B=\begin{bmatrix}0\\1\end{bmatrix}.$$

第一行就是 $\dot p=v$，第二行就是 $\dot v=u$。状态分量单位不同，所以矩阵系数隐含相应单位映射，不应把整个状态简单标成“米”。若传感器只测位置，观测 $z=Hx$，其中 $H=[1,0]$；这只是“从状态里取第一项”。噪声将在估计章节加入。

若每个采样间隔 $\Delta t$ 内加速度保持不变，积分得到精确的离散模型：

$$x_{k+1}=A_dx_k+B_du_k, \qquad A_d=\begin{bmatrix}1&\Delta t\\0&1\end{bmatrix}, \qquad B_d=\begin{bmatrix}\Delta t^2/2\\\Delta t\end{bmatrix}.$$

这是恒定输入假设下的精确结果；前向欧拉会遗漏位置里的 $u_k\Delta t^2/2$。例如 $p_k=0$、$v_k=1\ \mathrm{m/s}$、$u_k=2\ \mathrm{m/s^2}$、$\Delta t=0.1\ \mathrm s$，下一拍位置为 $0.11\ \mathrm m$，速度为 $1.2\ \mathrm{m/s}$。

特征值：沿某个特殊方向，一拍被放大多少？

对无输入离散系统 $x_{k+1}=A_dx_k$，若非零向量 $q$ 满足 $A_dq=\lambda q$，则 $q$ 是特征向量，$\lambda$ 是特征值。沿这个方向，矩阵不改变方向类型，只把幅值乘 $\lambda$。先看解耦例子（状态已归一化）：

$$A_d=\begin{bmatrix}0.5&0\\0&1.2\end{bmatrix}, \qquad x_0=\begin{bmatrix}1\\1\end{bmatrix}, \qquad x_k=\begin{bmatrix}0.5^k\\1.2^k\end{bmatrix}.$$

第一分量衰减，第二分量增长。有限维线性时不变离散系统渐近稳定的判据是所有特征值模长小于一；连续系统 $\dot x=Ax$ 则检查特征值实部小于零。两种判据不能互换。恰好在边界上不能直接宣布渐近稳定；例如无阻尼位置速度系统中的速度不会自动回零。

雅可比怎样接上 EKF？

上一课讲了局部切线。非线性观测 $z=h(x)$ 在当前估计 $\hat x$ 附近可写成 $h(\hat x+\delta x)\approx h(\hat x)+H\delta x$，其中 $H=\partial h/\partial x|_{\hat x}$。$\delta x$ 是小状态误差。线性位置观测 $h(x)=p$ 的雅可比就是 $[1,0]$；测距 $h(x)=\sqrt{p_x^2+p_y^2}$ 的雅可比为

$$H=\begin{bmatrix}p_x/\rho&p_y/\rho\end{bmatrix}, \qquad \rho=\sqrt{p_x^2+p_y^2}>0.$$

$p_x,p_y$ 是平面位置，$\rho$ 是到原点的距离，均用米。若当前位置为 $(3,4)\ \mathrm m$，$H=[0.6,0.8]$；向 $x$ 方向移动 $0.1\ \mathrm m$ 时，距离约增加 $0.06\ \mathrm m$。原点处该导数不存在，不能用除以极小数来假装模型正常。后续 EKF 的矩阵并非凭空出现，就是这些“输入小变化会造成多少输出变化”的系数。

自检：从方程到一拍预测

1. 00-L1：$f_1+f_2=8$ 与 $3f_1+3f_2=24$ 能否唯一确定两台推力？为什么？
2. 00-L2：三次恒力观测为 $1,2,6\ \mathrm N$，最小平方误差的恒力估计是多少？
3. 00-L3：离散特征值 $-0.5$ 和 $1.1$ 分别意味着什么？

展开逐步答案

00-L1：不能；第二式只是一式乘三，秩为一。令 $f_1=s$，则 $f_2=8-s$，任意 $s$ 都满足。00-L2：对 $J=\frac12\sum_{i=1}^3(y_i-\theta)^2$ 求导，得到 $3\theta-9=0$，所以估计为 $3\ \mathrm N$。00-L3：$-0.5$ 使该方向每拍变号且幅值减半；$1.1$ 每拍放大一成，存在不稳定方向。

完成这三课后再读第一章的坐标课。遇到矩阵时，先问每行输出是什么、每列输入是什么、单位能否对上，再考虑公式名称。