02.3｜非线性到15维误差状态：逐块写出 F、G、注入和重置

本课输出一套能手写的IMU误差传播，而不是“使用EKF”四个字。普通EKF把非线性运动函数在当前估计处泰勒展开，用雅可比传播局部误差；姿态却不能随意给旋转矩阵九个元素加高斯误差，因为它必须正交。ESKF让名义姿态保持在旋转群上，只把三维小旋转放进线性滤波。小误差是假设而非魔法：初始姿态差很大或错误观测造成大修正时，局部线性近似仍会失准。

需要补的数学只读这些： 泰勒展开与雅可比和状态空间模型。先能口头解释本页小例，再进入完整推导；不必预先背下矩阵公式。

先把“雅可比”翻译成一次小实验

导数回答“输入只改变一点，输出会改变多少”；雅可比只是多输入、多输出时把这些局部斜率排成表。第 $j$ 列只扰动第 $j$ 个输入，第 $i$ 行观察第 $i$ 个输出。元素 $H_{ij}$ 的单位为第 $i$ 个观测单位除以第 $j$ 个状态单位，绝不是所有元素都无量纲。

例如压力转深度的斜率 $\partial d/\partial\pi=1/(\rho g)$，单位 $\mathrm{m/Pa}$：压力多一点，深度增加多少可以直接相乘。DVL姿态列则问“朝向差一点，同一导航速度投影到机体后差多少”，单位是 $\mathrm{(m/s)/rad}$。这两件事数学上都是求局部斜率，物理上却不能混用单位。

为什么只用局部斜率？ 非线性曲线整体可能弯曲，当前点附近却可以用切线近似。我们并没有声称机器人一直做线性运动：名义状态仍使用完整非线性运动函数，只有“小误差如何传播”使用切线。若离当前点很远，切线不再贴近曲线，调小协方差不能让错误近似变正确。

名义、误差、真实：不是三台机器人

假设名义速度为 $0.30\,\mathrm{m/s}$，暂用来解释的真实速度为 $0.32\,\mathrm{m/s}$，真实误差便是 $0.02\,\mathrm{m/s}$。运行时真实值未知；滤波器只能从创新估计一个修正，例如 $\delta\hat v=0.01\,\mathrm{m/s}$。注入后名义速度变为 $0.31\,\mathrm{m/s}$，并不证明真实误差已经为零。

1. 传播名义量： 当前认为艇在哪里、朝哪里，按IMU推进。
2. 传播误差分布： 这些名义量可能错多少，按局部斜率推进协方差。
3. 由观测估计修正： 创新不是误差本身，而是误差经过测量函数后的表现，还混有噪声。
4. 注入并重置： 把已估出的修正吸收到名义量，误差均值重新以零为中心；剩余不确定度不消失，姿态相关协方差还必须换局部坐标。

微练习： 船已经转了 $90^\circ$，但姿态估计只差 $0.5^\circ$，这是否违反“小角误差”假设？注入后能否令 $P=0$？

普通EKF线性化究竟丢掉了什么

为什么线性KF不能直接接收一个非线性函数？

上一课要求观测满足 $z=Hx+\epsilon$，但DVL观测包含 $R^Tv$，姿态和速度相乘，不能用一个固定 $H$ 完整表示。EKF的办法不是把真实运动改成线性，而是在当前估计附近只描述小误差对结果的影响。

以普通向量状态为例，给当前估计 $\hat x$ 加小量 $\delta x$，一阶泰勒展开是

常数项推进名义状态，一次项传播误差，二阶及更高项在局部近似中舍去。测量函数同理：

从 $z=h(x)+\epsilon$ 两边减去 $h(\hat x)$，就得到创新的近似线性模型

这时才可以使用上一课的增益推导。比如 $h(x)=x^2$、$\hat x=2$，导数为4；$\delta x=0.01$ 时精确变化为 $4.0401-4=0.0401$，线性预测为 $0.04$，被丢掉的正是 $0.0001$。若误差变成1，线性化误差不再小，所以“用了EKF”并不意味着可以无视初始误差。姿态的局部加法必须用旋转组合替代，这是下面ESKF的起点。

固定约定，然后分别相减两套运动方程

名义状态与误差状态：同一个真实运动，两套描述

名义状态负责保存我们认为机器人正在怎样运动；误差状态只描述名义量还可能错多少。位置和速度处于欧氏空间，所以写成相加：$p_t=\hat p+\delta p$、$v_t=\hat v+\delta v$。下标 $t$ 在这里表示true，即真实值，不是采样时间。

旋转不同。直接给 $\hat R$ 的九个元素分别加误差会破坏 $R^TR=I$。我们声明本章始终采用右扰动：

$\hat R=\hat R_b^n$ 将机体分量转到NED；$\delta\theta\in\mathbb R^3$ 是名义机体系切空间中的旋转向量，单位rad。右乘表示这个小旋转在局部机体一侧表达。偏置仍用普通加法：$b_{a,t}=\hat b_a+\delta b_a$、$b_{g,t}=\hat b_g+\delta b_g$。

最终误差排序固定为

它的五组三维单位依次是m、m/s、rad、m/s²、rad/s。相应 $P$ 是 $15\times15$，不同块的单位不同。重力在本课设为已知的 $g^n=[0,0,g]^T$，因此没有额外重力误差三维。为减少长式中的重复，定义去偏输入 $\hat f=f_m-\hat b_a$ 和 $\hat\omega=\omega_m-\hat b_g$；这两个量还不是无噪声真值，只是名义积分所用的输入。

速度误差的三条来源，一项项相减得到

我们需要的不是 $\dot v$ 本身，而是 $\delta\dot v=\dot v_t-\dot{\hat v}$：真实与名义加速度为什么不同？真实模型用真实姿态、真实偏置并减去测量噪声；名义模型使用估计量。所以

第一步，重力因双方采用同一个已知向量而抵消。第二步，代入 $b_{a,t}=\hat b_a+\delta b_a$ 和 $R_t\approx\hat R(I+[\delta\theta]_\times)$：

把括号展开有六项。$\hat R\hat f$ 与末项抵消；$[\delta\theta]_\times\delta b_a$ 以及姿态误差乘噪声属于高阶小量，在线性近似中不保留。剩下

为了写成“矩阵乘未知误差”，把第一项叉乘调换次序。由于 $\delta\theta\times\hat f=-\hat f\times\delta\theta$，最终得到

现在三项的作用清楚了：第一项是姿态错误把比力投错方向；第二项是偏置扣错；第三项是随机比力误差。每项单位都为 $\mathrm{m/s^2}$。例如水平静止时 $\hat R=I$、$\hat f=[0,0,-9.81]^T$，若真实相对名义的俯仰误差为 $\delta\theta_y=0.01$，则第一项产生北向 $-0.0981\ \mathrm{m/s^2}$。先用这个简单例子检查负号，再编写全矩阵。

姿态误差为什么也会被名义角速度“搬动”？

即使误差旋转大小不变，其坐标也会随着名义机体系旋转。这是角误差传播中 $-[\hat\omega]_\times\delta\theta$ 的来源。先从两个真正的运动方程开始：$\dot R_t=R_t[\hat\omega-\delta b_g-n_g]_\times$，$\dot{\hat R}=\hat R[\hat\omega]_\times$。令 $D=[\delta\theta]_\times$，则 $R_t\approx\hat R(I+D)$。

对这个乘积求导：

将真实与名义姿态导数代入，左乘 $\hat R^T$，并略去 $D$ 乘偏置误差或噪声的二阶项：

两侧的 $[\hat\omega]_\times$ 抵消，移项得到

利用叉乘矩阵交换子的恒等式 $[a]_\times[b]_\times-[b]_\times[a]_\times=[a\times b]_\times$，前两项对应 $\delta\theta\times\hat\omega=-\hat\omega\times\delta\theta$。回到三维向量就是

这里每项单位都是rad/s。静止且角误差初值为零时，若真实陀螺偏置比估计多 $0.001\ \mathrm{rad/s}$，一秒后真实相对名义的角误差约为 $-0.001\ \mathrm{rad}$：名义积分因为少扣了正偏置而转得过多。这个手算与前面的负号一致。

把五条误差方程排成F：每个非零块都有来历

位置方程相减得到 $\delta\dot p=\delta v$。偏置采用随机游走：$\delta\dot b_a=n_{ba}$、$\delta\dot b_g=n_{bg}$。再加上刚推导的速度和姿态两行，共五组三维方程。令 $w=[n_a^T,n_g^T,n_{ba}^T,n_{bg}^T]^T$，按“误差项放$F$、独立噪声项放$G$”分组：

$F$ 为 $15\times15$；下面每个0、$I$ 都是 $3\times3$，行和列顺序均为 $p,v,\theta,b_a,b_g$：

逐块读而不是一次背下整张矩阵：

其余零块表达当前建模假设：例如无位置相关重力或流场时，速度导数不直接依赖位置；随机游走的偏置没有回拉项。如果改成一阶高斯—马尔可夫偏置模型，偏置对自身的对角块就不再为零。

$G$ 的列顺序是 $n_a,n_g,n_{ba},n_{bg}$，尺寸 $15\times12$：

它说明加速度计白噪声先进入速度，陀螺白噪声先进入角度，两种偏置驱动噪声分别进入偏置。没有直接进入位置的噪声列，不等于位置永远不受噪声影响；经过$F$中的 $F_{pv}$，速度噪声在一个区间内会继续传到位置，这会体现在精确离散过程协方差中。

连续白噪声到离散Q：不能只凭“乘dt”记忆

连续模型每秒都在积累误差，怎样变成一次离散更新？

冻结一个短区间内的 $F,G$，无噪声误差方程 $\delta\dot x=F\delta x$ 的解为

$\Phi$ 把旧误差映射到新误差；每个块的单位是输出误差单位除以输入误差单位，例如位置—速度块含秒，只有同类对角块或经过归一化后的矩阵才可统一视为无量纲。极短时间可以展开指数为 $\Phi\approx I+F\Delta t$，但大步长或强耦合不应随意截断。

为定义连续白噪声强度，写 $E[w(t)w(s)^T]=Q_c\delta(t-s)$。$\delta(t-s)$ 是狄拉克函数，不是误差状态。$Q_c$ 是谱密度矩阵，维度 $12\times12$。区间内时刻 $\tau$ 加入的噪声，还会被剩余时间的状态转移继续传播；对全部噪声影响的外积积分，得到

积分变量也可以写成“距离区间结束的剩余时间”，换元后与上式相同。每个被积项都是半正定矩阵，所以 $Q_d$ 也半正定。短步长近似冻结指数为单位阵时，$Q_d\approx GQ_cG^T\Delta t$。已知上一时刻的协方差，就按

进行预测。不是每一步都重置$P$，也不是只有传感器失效才增加$Q$；正常运动中未知噪声同样存在。

用一维速度检查单位：若加速度白噪声谱密度为 $q_a=0.01\ \mathrm{m^2/s^3}$，$\Delta t=0.1\ \mathrm s$，速度增量方差为 $q_a\Delta t=0.001\ \mathrm{m^2/s^2}$。若给你的却是每次采样的加速度方差 $\sigma_a^2=0.01\ \mathrm{m^2/s^4}$，单步速度增量方差是 $\sigma_a^2\Delta t^2=0.0001\ \mathrm{m^2/s^2}$。两式没有矛盾：它们的输入噪声参数单位不同。

观测更新后为什么还必须重置协方差

误差KF算出的不是最终姿态：先把修正注入名义状态

设某条观测给出 $r=z-h(\hat x)$，局部导数为$H$。先按上一课计算 $S=HP^-H^T+R_{\mathrm{obs}}$ 和 $K=P^-H^TS^{-1}$。误差先验均值为零时，观测后的误差均值是

注意 $R_{\mathrm{obs}}$ 是观测噪声协方差，与姿态矩阵 $\hat R$ 不同；代码命名应主动区分。$\delta\hat x$ 的五组分量分别有位置、速度、角度和两类偏置的单位。用Joseph式得到 $P_{\mathrm{upd}}$ 后，欧氏量直接相加：

姿态则按最初声明的右扰动进行组合：

例如 $\delta\hat\theta=[0,0,0.02]^T$，表示名义机体下轴附近约 $1.146^\circ$ 的局部修正。它不等于在任意姿态下把欧拉yaw直接加0.02；欧拉角的耦合取决于当前姿态。注入后，名义状态吸收了已经估计出的误差，后续误差均值可以重新以零为中心，但还不能忘记协方差也需要换坐标。

重置不是清零不确定度，而是换一个局部坐标原点

设本次姿态注入量为 $a=\delta\hat\theta$。真实旋转不能因为我们修改估计而改变，所以注入前后必须满足

代入 $\hat R_{\mathrm{new}}=\hat R_{\mathrm{old}}\operatorname{Exp}([a]_\times)$，左乘其逆，得到真正的误差坐标变换：

旋转不交换，所以新误差并不严格等于旧误差减 $a$。用乘积对数的首个交换子展开：

我们要传播的是更新后在 $\delta\theta_{\mathrm{old}}=a$ 附近的残余随机偏差。对旧误差求导，精确的一阶局部导数为SO(3)右雅可比：

$\alpha$ 是修正转角，单位rad；$J_r$ 是 $3\times3$。当 $\alpha\to0$，用 $1-\cos\alpha\approx\alpha^2/2$、$\alpha-\sin\alpha\approx\alpha^3/6$，得到 $J_r\approx I-\tfrac12[a]_\times+\tfrac16[a]_\times^2$。若只留到a的一次，常见形式就是 $I-\tfrac12[a]_\times$。

其余欧氏误差只是减去一个已知修正量，对旧误差的导数为$I$，因此完整重置矩阵是

使用协方差的坐标变换规则，最后执行

给 $a=[0,0,0.02]^T$，一阶近似的左上 $2\times2$ 为 $\begin{bmatrix}1&0.01\\-0.01&1\end{bmatrix}$。负号来自右扰动，不可换成左扰动的正号。并且必须对整个$P$做左右变换，不能只处理姿态对角块：姿态与速度、偏置的互协方差也属于同一个局部坐标体系。下一课的C++有限差分会直接检查这个导数。

实现流程、源码阅读与练习

先独立手写：15维误差传播与一次压力修正

1. 使用标准库二维数组写出$F$与$G$，按表格逐块填入，禁止手动录入225个没有语义的数字。
2. 写一个名义状态预测，再用 $\Phi\approx I+F\Delta t$、$Q_d\approx GQ_cG^T\Delta t$ 更新$P$。
3. 融合已经修正到IMU参考点的深度观测，执行Joseph、注入、reset。最后检查旋转正交性、$P$对称性和非负对角项。非负对角只是一项基本检查，不等于完整半正定证明。
4. 独立说明为何把误差均值归零不代表将$P$归零。

#include <vector>
#include <cmath>
#include <cassert>
#include <algorithm>
#include <iostream>
using V=std::vector<double>;
using M=std::vector<V>;
M zero(int r,int c){return M(r,V(c,0.));}
M eye(int n){M a=zero(n,n);for(int i=0;i<n;++i)a[i][i]=1;return a;}
M tr(const M&a){M b=zero(a[0].size(),a.size());for(size_t i=0;i<a.size();++i)for(size_t j=0;j<a[0].size();++j)b[j][i]=a[i][j];return b;}
M mul(const M&a,const M&b){M c=zero(a.size(),b[0].size());for(size_t i=0;i<a.size();++i)for(size_t k=0;k<b.size();++k)for(size_t j=0;j<b[0].size();++j)c[i][j]+=a[i][k]*b[k][j];return c;}
V mv(const M&a,const V&b){V c(a.size());for(size_t i=0;i<a.size();++i)for(size_t j=0;j<b.size();++j)c[i]+=a[i][j]*b[j];return c;}
M skew(const V&a){return {{0,-a[2],a[1]},{a[2],0,-a[0]},{-a[1],a[0],0}};}
V cross(const V&a,const V&b){return mv(skew(a),b);}
double norm(const V&a){double s=0;for(double v:a)s+=v*v;return std::sqrt(s);}
M Exp(const V&a){
    const double t=norm(a);M A=skew(a),A2=mul(A,A),R=eye(3);
    const double c1=t<1e-6?1-t*t/6:std::sin(t)/t;
    const double c2=t<1e-6?.5-t*t/24:(1-std::cos(t))/(t*t);
    for(int i=0;i<3;++i)for(int j=0;j<3;++j)R[i][j]+=c1*A[i][j]+c2*A2[i][j];
    return R;
}
M Jr(const V&a){
    const double t=norm(a);M A=skew(a),A2=mul(A,A),J=eye(3);
    const double c1=t<1e-6?.5-t*t/24:(1-std::cos(t))/(t*t);
    const double c2=t<1e-6?1./6-t*t/120:(t-std::sin(t))/(t*t*t);
    for(int i=0;i<3;++i)for(int j=0;j<3;++j)J[i][j]+=-c1*A[i][j]+c2*A2[i][j];
    return J;
}
void block(M&A,int row,int col,const M&B,double scale=1){
    for(size_t i=0;i<B.size();++i)for(size_t j=0;j<B[0].size();++j)A[row+i][col+j]=scale*B[i][j];
}

int main(){
    // 名义状态p、v在NED；R从FRD到NED；ba/bg在FRD。
    V p(3),v(3),ba(3),bg(3),fm{0,0,-9.81},wm(3);
    M R=eye(3),P=eye(15);for(int i=0;i<15;++i)P[i][i]=.01;
    const double dt=.01;
    V f(3),w(3);for(int i=0;i<3;++i){f[i]=fm[i]-ba[i];w[i]=wm[i]-bg[i];}
    M F=zero(15,15),G=zero(15,12);
    block(F,0,3,eye(3));block(F,3,6,mul(R,skew(f)),-1);
    block(F,3,9,R,-1);block(F,6,6,skew(w),-1);block(F,6,12,eye(3),-1);
    block(G,3,0,R,-1);block(G,6,3,eye(3),-1);
    block(G,9,6,eye(3));block(G,12,9,eye(3));
    M Phi=eye(15),Qc=zero(12,12);
    for(int i=0;i<15;++i)for(int j=0;j<15;++j)Phi[i][j]+=F[i][j]*dt;
    // 白噪声谱密度：此处演示用，不是传感器标定值。
    for(int i=0;i<12;++i)Qc[i][i]=(i<3?1e-3:(i<6?1e-5:1e-8));
    M Qd=mul(mul(G,Qc),tr(G));P=mul(mul(Phi,P),tr(Phi));
    for(int i=0;i<15;++i)for(int j=0;j<15;++j)P[i][j]+=Qd[i][j]*dt;
    V acceleration=mv(R,f);acceleration[2]+=9.81;
    for(int i=0;i<3;++i){p[i]+=v[i]*dt+.5*acceleration[i]*dt*dt;v[i]+=acceleration[i]*dt;w[i]*=dt;}
    R=mul(R,Exp(w));
    // 深度观测已经校正到IMU参考点：H只有位置z列为1。
    const double z=.02,Robs=1e-4,r=z-p[2],S=P[2][2]+Robs;
    V K(15),dx(15);for(int i=0;i<15;++i){K[i]=P[i][2]/S;dx[i]=K[i]*r;}
    M A=eye(15);for(int i=0;i<15;++i)A[i][2]-=K[i];
    M Pu=mul(mul(A,P),tr(A));
    for(int i=0;i<15;++i)for(int j=0;j<15;++j)Pu[i][j]+=K[i]*Robs*K[j];
    V angle(3);for(int i=0;i<3;++i){p[i]+=dx[i];v[i]+=dx[3+i];angle[i]=dx[6+i];ba[i]+=dx[9+i];bg[i]+=dx[12+i];}
    R=mul(R,Exp(angle));M J=eye(15);block(J,6,6,Jr(angle));P=mul(mul(J,Pu),tr(J));
    // 重置的是误差均值，P必须保留。
    std::fill(dx.begin(),dx.end(),0.);
    assert(p[2]>0 && p[2]<z && P[2][2]>0);
    M RtR=mul(tr(R),R);for(int i=0;i<3;++i)for(int j=0;j<3;++j)assert(std::abs(RtR[i][j]-(i==j))<1e-10);
    for(int i=0;i<15;++i){assert(P[i][i]>=0);for(int j=0;j<15;++j)assert(std::abs(P[i][j]-P[j][i])<1e-10);}
    std::cout<<"depth="<<p[2]<<" variance="<<P[2][2]<<'\n';
}