02.2｜从两个不确定数字推导卡尔曼滤波，而不是背五条公式

输入是预测值及其方差、独立观测及其方差；输出是融合后的估计与剩余不确定度。本课从标量推导到矩阵。假设预测误差与当前观测噪声零均值、互不相关，协方差已知。高斯假设让结果等于后验均值；即使不是高斯，这一推导仍给出线性无偏估计中的最小均方误差解。这里的误差定义为真实值减估计值，不是控制中的目标减实际。

符号／对象	含义与维度	单位与约定
$x,\hat x^-,\hat x^+$	真实、预测、更新后状态	本例速度为m/s；上标不是数的正负
$z,r,H$	$m$ 维观测、创新、 $m\times n$ 观测矩阵	$r=z-H\hat x^-$ ； $H$ 负责状态到观测的单位映射
$P^-,P^+,R$	预测、后验、观测协方差	对角项是单位平方，非标准差
$K,S$	$n\times m$ 增益、 $m\times m$ 创新协方差	$K$ 为状态/观测单位， $S$ 为观测单位平方
$A,B,u,Q$	状态转移、输入矩阵、已知输入、过程增量协方差	$Q$ 与 $P$ 同单位，不是质量q

需要补的数学只读这些： 矩阵如何乘向量和平方目标如何最小化。先能口头解释本页小例，再进入完整推导；不必预先背下矩阵公式。

开始推导前：均值、方差、协方差究竟在记什么

一次测量只有一个数，但“这个数可能错多少”需要另一本账。概率不是质量评分，而是模型给不同可能结果分配的权重；期望 $E[\cdot]$ 是按这些权重求平均，不是取最大概率的那一个结果。均值表示中心，方差表示围绕中心的平方散布，标准差才恢复原来的物理单位。

用一个可以手算的速度误差模型建立直觉：误差 $e$ 等概率取 $-0.2$ 与 $+0.2\,\mathrm{m/s}$ 。目的不是声称真实噪声只取两个值，而是看懂平均运算：

\mu_e=E[e]=\tfrac12(-0.2)+\tfrac12(0.2)=0\,\mathrm{m/s}.

\operatorname{Var}(e)=E[(e-\mu_e)^2]=0.04\,\mathrm{m^2/s^2},\qquad \sigma_e=0.2\,\mathrm{m/s}.

均值为零不等于每次误差为零：正负误差互相抵消了。
平方避免抵消，所以方差记录误差规模；把标准差误填为方差会改变信任程度。
如果误差均值不为零， $E[e^2]=\operatorname{Var}(e)+\mu_e^2$ 。下文把 $E[e^2]$ 写作方差，依赖零均值假设；偏置未处理时不能悄悄使用这个等号。

协方差是在记“两项误差是否一起偏”。 两个零均值误差的协方差为 $E[e_1e_2]$ ，单位是两者单位的乘积。若位置误差为正时速度误差也常为正，乘积平均为正；若常一正一负，则为负。负协方差不是负方差，非对角元素允许为负。协方差矩阵的对角是各自方差，非对角保存这种关联。噪声是误差来源，协方差是我们对其统计规模的描述，两者不是同一件东西。

不带矩阵，先猜融合结果。 预测速度为 $0.30\,\mathrm{m/s}$ ，观测为 $0.22\,\mathrm{m/s}$ 。预测标准差 $0.2\,\mathrm{m/s}$ ，观测标准差 $0.5\,\mathrm{m/s}$ ：预测更可靠，所以新值应更接近 $0.30$ 。下一节推导的 $K$ 正是“从预测走向观测的比例”，不是传感器质量百分比。

两只时钟动作：预测让时间前进，更新让知识改变

在时刻 $k-1$ 融合完毕，保存 $\hat x_{k-1}^+,P_{k-1}^+$ 。拿这一段已知输入预测到时刻 $k$ ，得到 $\hat x_k^-,P_k^-$ ；等时刻 $k$ 的观测到来，再得到 $\hat x_k^+,P_k^+$ 。上标 $-$ 与 $+$ 是“本次观测使用前／后”，不表示负数／正数。更新前后描述的是同一物理时刻，不是又走了一个采样周期。

例如 $1.00\,\mathrm s$ 到 $1.02\,\mathrm s$ 的IMU推进一次，随后用 $1.02\,\mathrm s$ 的DVL改正同一状态；若没有新DVL，就只有预测，不能拿上一帧反复更新。过程协方差 $Q$ 给新的一段模型误差记账，观测协方差 $R$ 给这一次读数误差记账；它们都不是为了“曲线好看”而随意填的平滑旋钮。

微练习： 两个预测具有相同均值，但方差分别是 $0.01$ 与 $1\,\mathrm{m^2/s^2}$ ；收到同一个新速度观测，哪个应该移动得更多？

概念答案：中心相同，信心不同

后者。固定正的 $R$ 时， $K=P^-/(P^-+R)$ 随先验方差增大而增大。均值相同不代表知识相同，所以状态估计必须同时保存均值与协方差。到标量KF练习用相同观测验证，再到二维KF练习观察位置与速度之间的互协方差。

标量：为什么权重是方差之比

1．先提出一个可调整的估计器，而不是直接写增益

同一瞬间，预测告诉我们速度约为 $\hat x^-$ ，DVL告诉我们速度约为 $z$ 。为什么不取简单平均？因为二者精度不同。我们先允许估计器沿着“观测与预测的差”走任意比例 $K$ ：

\hat x^+=\hat x^-+K(z-\hat x^-).

量	含义	本例单位
$x$	不能直接知道的真实速度	$\mathrm{m/s}$
$\hat x^-,\hat x^+$	更新前、更新后的速度估计	$\mathrm{m/s}$
$z=x+\epsilon$	DVL测量； $\epsilon$ 是零均值测量噪声	$\mathrm{m/s}$
$K$	我们接下来要优化的权重	无量纲
$e^-=x-\hat x^-$	预测误差，是真实减估计	$\mathrm{m/s}$

$K=0$ 表示不动预测； $K=1$ 表示完全采用观测。我们不预先规定哪个更好，而是计算更新后还剩多少均方误差。把新估计代入 $e^+=x-\hat x^+$ ：

e^+=x-\hat x^--K(z-\hat x^-).

因为 $z-\hat x^-=(x-\hat x^-)+\epsilon=e^-+\epsilon$ ，所以

e^+=(1-K)e^--K\epsilon.

这一步说明新误差有两个来源：保留下来的预测误差，以及观测噪声被增益放大的部分。不能只顾把预测误差系数变小，因为 $K$ 越大，测量噪声贡献也越大。定义 $P^-=E[(e^-)^2]$ 、 $R=E[\epsilon^2]$ ，两者是方差，单位为 $\mathrm{m^2/s^2}$ 。展开平方：

P^+(K)=(1-K)^2P^-+K^2R-2K(1-K)E[e^-\epsilon].

假设当前测量噪声与预测误差互不相关，最后一项为零，才得到待优化的二次函数：

P^+(K)=(1-K)^2P^-+K^2R.

这也是为什么重复融合同一旧样本会有问题：第一次融合后，预测已经含有它的噪声，第二次再声称 $E[e^-\epsilon]=0$ 就错了。

2．让均方误差最小，增益自然出现

现在只剩一个标量未知数 $K$ 。对上式求导时， $P^-$ 与 $R$ 都是已知常量。链式法则给出

\frac{dP^+}{dK}=-2(1-K)P^-+2KR.

令导数等于零，先展开，再把所有含 $K$ 的项移到一侧：

-2P^-+2KP^-+2KR=0 \quad\Longrightarrow\quad K(P^-+R)=P^-.

只要 $P^-+R>0$ ，便有

K=\frac{P^-}{P^-+R}.

二阶导数是 $2(P^-+R)>0$ ，所以这确实是最小值，不只是一个驻点。如果预测方差远大于观测方差， $K$ 接近1；反之接近0。特别地，二者相同才得到简单平均的 $K=1/2$ 。

再把最优 $K$ 代回，而不是把后验方差凭经验减小。注意 $1-K=R/(P^-+R)$ ：

\begin{aligned} P^+&=\frac{R^2P^-+(P^-)^2R}{(P^-+R)^2}\\ &=\frac{P^-R(P^-+R)}{(P^-+R)^2}\\ &=\frac{P^-R}{P^-+R}. \end{aligned}

两边取倒数，有 $1/P^+=1/P^-+1/R$ 。这说明独立高斯信息的精度相加。极限也很有用： $R\to\infty$ 时， $K\to0$ 、 $P^+\to P^-$ ；非常差的观测不会使状态突然变得精确。 $R\to0$ 且测量模型正确时， $K\to1$ ，直接测到的这一维不确定性趋近零。

项目质量q=50的完整手算

3．把每个单位带入同一帧DVL算例

取项目质量分 $q=50$ 。先由质量得到速度标准差，注意这里的 $q$ 不是四元数，也不是过程噪声 $Q$ ：

\sigma_v=0.9-0.8\frac{50}{100}=0.5\ \mathrm{m/s},\qquad R=\sigma_v^2=0.25\ \mathrm{m^2/s^2}.

预测为 $\hat v^-=0.30\ \mathrm{m/s}$ 、 $P^-=0.04\ \mathrm{m^2/s^2}$ ，观测为 $z=0.22\ \mathrm{m/s}$ 。创新使用“观测减预测”：

r=z-\hat v^-=0.22-0.30=-0.08\ \mathrm{m/s}.

负创新意味着预测过大，应向下修正。创新自身也不确定，因为它包含预测误差和观测噪声；二者独立，所以创新方差是

S=P^-+R=0.29\ \mathrm{m^2/s^2},\qquad K=\frac{0.04}{0.29}=0.137931.

先乘再加：速度修正量 $Kr=-0.0110345\ \mathrm{m/s}$ ，因此

\hat v^+=0.2889655\ \mathrm{m/s},\qquad P^+=\frac{0.04\times0.25}{0.29}=0.0344828\ \mathrm{m^2/s^2}.

检查三个结果：新估计应位于预测与观测之间；质量较差时移动应较小；后验方差应小于原方差但不能负。只比较速度是否接近DVL而不看方差，会误以为滤波器“响应太慢”，其实它正在按给定的不确定度正确权衡。

矩阵：一条速度观测为何能修正偏置

4．为什么多维预测要左右各乘一次状态转移

令 $x_k\in\mathbb R^n$ 为状态， $u_k\in\mathbb R^l$ 为已知输入。离散模型为

x_k=A_kx_{k-1}+B_ku_k+w_k.

$A_k$ 为 $n\times n$ 状态转移， $B_k$ 为 $n\times l$ 输入矩阵， $w_k$ 是零均值过程增量， $Q_k=E[w_kw_k^T]$ 。过程噪声描述这一段模型无法精确预测的变化，不是当前DVL的测量误差。将旧状态用估计替换，并使用 $E[w_k]=0$ ，均值预测是

\hat x_k^-=A_k\hat x_{k-1}^++B_ku_k.

两式相减，已知输入正好抵消，得到 $e_k^-=A_ke_{k-1}^++w_k$ 。协方差的定义是误差外积的期望，所以

P_k^-=E[(A_ke_{k-1}^++w_k)(A_ke_{k-1}^++w_k)^T].

展开有四项：旧误差外积、过程噪声外积、两项交叉项。假设旧误差与新过程噪声独立，两项交叉项为零，剩下

P_k^-=A_kP_{k-1}^+A_k^T+Q_k.

左右各乘一次，是因为协方差既要变换外积左边的向量，也要变换右边的转置向量。只写 $AP$ 会破坏对称性与单位。

当前传感器只看状态的一部分，写为 $z_k=H_kx_k+\epsilon_k$ 。 $z_k\in\mathbb R^m$ ， $H_k\in\mathbb R^{m\times n}$ ， $R_k=E[\epsilon_k\epsilon_k^T]$ 。例如 $x=[p,v]^T$ 、速度观测时 $H=[0,1]$ ； $H$ 的作用不是“告诉滤波器有个传感器”，而是明确状态如何生成可与读数比较的预测。

5．从误差外积完整推导矩阵增益与Joseph形式

先沿用标量估计器，只把 $K$ 变成 $n\times m$ 矩阵： $\hat x^+=\hat x^-+Kr$ ，其中 $r=z-H\hat x^-$ 。把 $z=Hx+\epsilon$ 代入可得 $r=He^-+\epsilon$ ，于是

e^+=(I-KH)e^--K\epsilon.

$KH$ 是 $n\times n$ ；它描述观测修正了哪些状态误差方向。对外积取期望，且使用不相关假设，直接得到

P^+=(I-KH)P^-(I-KH)^T+KRK^T.

这就是Joseph形式，不需要等到知道最优增益才成立。第一项是剩余先验误差，第二项是引入的测量噪声。两项都是某个协方差的线性变换，因此在精确算术下均为半正定；相加仍半正定。

创新方差也从定义得到，而非一个额外猜测：

\begin{aligned} S&=E[rr^T]\\ &=E[(He^-+\epsilon)(He^-+\epsilon)^T]\\ &=HP^-H^T+R. \end{aligned}

$S$ 的尺寸为 $m\times m$ 。接下来为了优化 $K$ ，将Joseph式逐项展开：

\begin{aligned} P^+={}&P^--KHP^--P^-H^TK^T\\ &+KHP^-H^TK^T+KRK^T. \end{aligned}

后两项共有左右的 $K$ ，因此合并为 $KSK^T$ 。取迹得到标量目标 $J(K)=\operatorname{tr}(P^+)$ 。利用迹的循环性质及矩阵转置不改变迹，两个线性项相同：

J(K)=\operatorname{tr}(P^-)-2\operatorname{tr}(KHP^-)+\operatorname{tr}(KSK^T).

为什么对矩阵求导会得到下面的结果？可从单个元素 $K_{ab}$ 看：线性项对它的导数为 $-2(HP^-)_{ba}$ ；二次项为 $\sum_{i,j,l}K_{ij}S_{jl}K_{il}$ ，求导分别打到左右两个 $K$ ，由于 $S$ 对称，两份相同，合成 $2(KS)_{ab}$ 。汇总为

\frac{\partial J}{\partial K}=-2P^-H^T+2KS.

令它为零得到 $KS=P^-H^T$ 。若 $S$ 正定，唯一最优增益为

K=P^-H^TS^{-1}.

“最优”还可以不依赖记忆求导规则来检查。记 $K_*=P^-H^TS^{-1}$ ，把平方配齐，有

P^+(K)=P^+(K_*)+(K-K_*)S(K-K_*)^T.

最后一项半正定，所以任何其他增益都不会让协方差在某个方向比最优值更小。实际程序不计算 $S^{-1}$ ，而解方程 $SK^T=HP^-$ ；对称正定时可用Cholesky分解。小型标量观测只需除以一个正数。

6．两维状态的一帧数值：观测速度，修正偏置

令 $x=[v,b_a]^T$ ， $H=[1,0]$ ， $P^-=\begin{bmatrix}0.04&-0.01\\-0.01&0.09\end{bmatrix}$ 。各元素单位不相同：左上为速度方差，右下为加速度偏置方差，非对角为二者单位的乘积。取 $R=0.25$ ，则 $S=0.29$ ， $P^-H^T=[0.04,-0.01]^T$ 。所以 $K=[0.137931,-0.0344828]^T$ 。创新 $-0.08\ \mathrm{m/s}$ 使速度下降 $0.0110345\ \mathrm{m/s}$ ，使偏置上升 $0.00275862\ \mathrm{m/s^2}$ 。

这符合物理：若过去惯性速度比DVL大，可能是加速度偏置扣少了；把偏置估计增大，以后积分的加速度就会减小。但这个判断来自历史传播形成的互协方差，不是这帧DVL直接测到了偏置。

假设状态是 $x=[v,b_a]^T$ ，观测只看速度， $H=[1,0]$ 。增益第一项含速度方差，第二项含偏置与速度的互协方差。长期传播中偏置造成速度漂移，二者建立相关性，于是速度创新也能改变偏置。这不是凭一帧速度“直接测到”偏置，而是运动模型把过去证据连接了起来。不同状态单位不同，互协方差的单位就是两个状态单位的乘积。

可运行教学KF：先跑一维，再扩展状态

先独立手写：不要跳过P的更新

完成标量 update(x,P,z,R)，先用 $q=50$ 算例核对，再把 $R$ 改成0.01。
完成两状态、单观测的Joseph更新，使用上面的速度—偏置例；输出两个状态和四个协方差元素。
给同一个观测重复更新十次，解释为什么数值 $P$ 会变小，但在现实中这种“精度提升”是假的。

在线练习：标量KF；进阶的匀速位置—速度KF使用状态 $[p,v]^T$ ，不要与本课例子的 $[v,b_a]^T$ 混淆。

参考实现与运行检查（C++17，先完成练习再展开）

用 g++ -std=c++17 -O2 main.cpp -o main && ./main 编译运行。以下是离线教学实现，不连接传感器或执行器。

#include <array>
#include <cassert>
#include <cmath>
#include <iomanip>
#include <iostream>
struct Scalar { double x, P; };
void scalar_update(Scalar& s, double z, double R) {
    assert(s.P>=0 && R>0);
    const double prior=s.P, r=z-s.x, S=prior+R, K=prior/S;
    s.x+=K*r;
    s.P=(1-K)*(1-K)*prior+K*K*R; // Joseph，不复用更新后的P
}
using V=std::array<double,2>;
using M=std::array<std::array<double,2>,2>;
void update(V& x, M& P, const V& H, double z, double R) {
    const M old=P;
    V PH{}, K{};
    for(int i=0;i<2;++i) for(int j=0;j<2;++j) PH[i]+=old[i][j]*H[j];
    double S=R, r=z;
    for(int i=0;i<2;++i) { S+=H[i]*PH[i]; r-=H[i]*x[i]; }
    assert(S>0);
    M A{}, next{};
    for(int i=0;i<2;++i) {
        K[i]=PH[i]/S;
        x[i]+=K[i]*r;
        for(int j=0;j<2;++j) A[i][j]=(i==j?1.:0.)-K[i]*H[j];
    }
    for(int i=0;i<2;++i) for(int j=0;j<2;++j) {
        for(int a=0;a<2;++a) for(int b=0;b<2;++b)
            next[i][j]+=A[i][a]*old[a][b]*A[j][b];
        next[i][j]+=K[i]*R*K[j];
    }
    P=next;
}
int main() {
    Scalar s{.30,.04}; scalar_update(s,.22,.25);
    assert(std::abs(s.x-.2889655172413793)<1e-12);
    V x{.30,0}; M P{{{.04,-.01},{-.01,.09}}};
    update(x,P,V{1,0},.22,.25);
    assert(std::abs(x[1]-.0027586206896551726)<1e-12);
    assert(P[0][0]>=0 && P[1][1]>=0);
    assert(P[0][0]*P[1][1]-P[0][1]*P[1][0]>=-1e-12);
    std::cout<<std::setprecision(10)<<s.x<<' '<<s.P<<'\n';
    std::cout<<x[0]<<' '<<x[1]<<'\n';
}

算法顺序：按真实时间间隔做预测；每个新观测先构造 $H$ 、 $R$ 与创新；求 $S$ ；检查有效性与创新门限；通过后求增益、改均值、用Joseph式改协方差。门限拒绝观测时不要仍然缩小 $P$ 。对称化只消除浮点舍入的不对称，不能修复错误噪声模型、负方差或错误雅可比。

ArduSub映射、练习与答案要点

补充练习答案与阅读提示

在项目链路里，writeBodyFrameOdom负责封装与时间安排，FuseBodyVel才是观测创新、门限与状态修正所在。项目笔记的创新写成预测减观测，而本教材坚持观测减预测；相应修正符号必须一起变化，不能只翻转日志符号。练习一：把 $R$ 改成0.01，算新 $K$ 与速度。答案： $K$ =0.8，速度0.236 m/s，后验方差0.008。练习二：取 $P=\begin{bmatrix}0.04&-0.01\\-0.01&0.09\end{bmatrix}$ 、 $H=[1,0]$ 、 $R=0.25$ ，算偏置修正。答案：第二个增益−0.034483，乘创新−0.08得到偏置增加0.002759（按偏置状态单位）。精读 Solà 第6节时，先把每个更新符号映射回本课，再进入非线性几何。TB02-S1

对照真实C++：公式等价，不代表数值实现必须写成同一种形式

在本模块DVL源码精读所述同一上游提交中，FuseBodyVel 使用稀疏结构构造协方差修正，并检查方差是否会变负。下面为 AP_NavEKF3_PosVelFusion.cpp 第1854—1865行的独立短摘录；ArduPilot贡献者，GNU GPL v3，许可与版本来源同上。

                        P[i][j] = P[i][j] - KHP[i][j];

                ForceSymmetry();
                ConstrainVariances();

KHP 是 $KHP^-$ 的矩阵乘积，因此第一行对应简式 $P^+=(I-KH)P^-$ 。把最优关系 $KS=P^-H^T$ 代回Joseph展开式，可以证明两者在精确算术下等价。但简式包含相近数相减，舍入可能破坏对称或非负性；后两行分别强制对称并约束方差。上游围绕性能和完整状态实现了自己的数值保护，本课手写版本采用Joseph，便于直接看到“两个半正定项相加”的来源。

不要从这一行推断“协方差不用考虑测量噪声”： $R$ 已经进入增益 $K$ 和创新方差 $S$ ，简式只是代数消元之后没有显式出现它。反过来，也不要在更新 $P$ 后用新 $P$ 重新计算同一帧的 $K$ ；所有这一帧的增益都应由更新前的先验构造。