← 返回博客首页
ESKF 观测误差与 H 矩阵：从观测模型到 Jacobian
这是 ESKF 系列的第三篇。上一篇文章已经在固定的坐标系和右侧姿态扰动下推导了 IMU 误差动力学矩阵 F F F G G G H H H
摘要
ESKF 里的观测更新常被压缩成几行代码：计算残差，构造 H H H
本文固定上一篇文章的约定。R W B R_{WB} R W B B B B W W W
R W B = R ^ W B Exp ( [ δ θ ] × ) , R_{WB}=\hat R_{WB}\operatorname{Exp}([\delta\theta]_\times), R W B = R ^ W B Exp ([ δ θ ] × ) , 误差状态排列为
δ x = [ δ θ δ p δ v δ b g δ b a ] . \delta x=
\begin{bmatrix}
\delta\theta\\
\delta p\\
\delta v\\
\delta b_g\\
\delta b_a
\end{bmatrix}. δ x = δ θ δ p δ v δ b g δ b a . 文章先从一般观测模型 z = h ( X ) + n z=h(\mathcal X)+n z = h ( X ) + n H H H
读完之后，看到一个新的传感器模型，可以按同一套步骤自己写出 H H H
关键词： ESKF；观测模型；观测残差；H 矩阵；误差状态；姿态 Jacobian；LiDAR 点到平面
1. 先回答：下一篇为什么是观测误差和 H H H
上一篇推导了误差状态怎样随 IMU 传播：
δ x ˙ = F δ x + G w . \dot{\delta x}=F\delta x+Gw. δ x ˙ = F δ x + Gw . 它回答的是：在两次外部观测之间，误差会怎样变大、怎样互相影响。
当相机、GNSS、轮速计或 LiDAR 的新数据到来时，滤波器需要回答另一个问题：
当前这条观测，对误差状态中的哪些方向敏感？
这个问题由观测 Jacobian H H H m m m 15 15 15
H ∈ R m × 15 . H\in\mathbb R^{m\times15}. H ∈ R m × 15 . 它的每一列对应一个误差方向，每一行对应一个观测分量。某个 block 为零，表示在当前观测模型和当前线性化点附近，该观测对相应误差的一阶变化不敏感；某个 block 非零，表示这个误差会改变预测观测。
可以把 F F F H H H
矩阵
它回答的问题
典型形式
F F F 误差怎样随时间传播
δ x ˙ = F δ x + G w \dot{\delta x}=F\delta x+Gw δ x ˙ = F δ x + Gw
H H H 观测怎样感受到误差
r ≈ H δ x + n r\approx H\delta x+n r ≈ H δ x + n
F F F H H H
2. 先把约定固定下来
2.1 状态、坐标系和姿态误差
本文使用的完整状态为
X = ( R W B , p W , v W , b g , b a ) . \mathcal X=(R_{WB},p_W,v_W,b_g,b_a). X = ( R W B , p W , v W , b g , b a ) . 其中 p W p_W p W v W v_W v W b g b_g b g b a b_a b a
q W = R W B q B , q B = R W B ⊤ q W . q_W=R_{WB}q_B,
\qquad
q_B=R_{WB}^{\top}q_W. q W = R W B q B , q B = R W B ⊤ q W . 姿态采用右侧小扰动：
R W B = R ^ W B Exp ( [ δ θ ] × ) . R_{WB}=\hat R_{WB}\operatorname{Exp}([\delta\theta]_\times). R W B = R ^ W B Exp ([ δ θ ] × ) . 因为误差在名义姿态右侧，δ θ \delta\theta δ θ
p W = p ^ W + δ p , v W = v ^ W + δ v , b g = b ^ g + δ b g , b a = b ^ a + δ b a . \begin{aligned}
p_W&=\hat p_W+\delta p,\\
v_W&=\hat v_W+\delta v,\\
b_g&=\hat b_g+\delta b_g,\\
b_a&=\hat b_a+\delta b_a.
\end{aligned} p W v W b g b a = p ^ W + δ p , = v ^ W + δ v , = b ^ g + δ b g , = b ^ a + δ b a . 因此，后文所有观测 Jacobian 都按以下列顺序排列：
H = [ H θ H p H v H b g H b a ] . H=\begin{bmatrix}H_\theta&H_p&H_v&H_{b_g}&H_{b_a}\end{bmatrix}. H = [ H θ H p H v H b g H b a ] . 每个 block 的列数都是 3 3 3
2.2 观测噪声不要和旋转矩阵混用同一个符号
观测模型写成
z = h ( X ) + n , z=h(\mathcal X)+n, z = h ( X ) + n , 其中 z ∈ R m z\in\mathbb R^m z ∈ R m h ( X ) h(\mathcal X) h ( X ) n n n R W B R_{WB} R W B Σ z \Sigma_z Σ z
n ∼ N ( 0 , Σ z ) . n\sim\mathcal N(0,\Sigma_z). n ∼ N ( 0 , Σ z ) . 例如，GNSS 位置观测的 Σ z \Sigma_z Σ z 3 × 3 3\times3 3 × 3 Σ z \Sigma_z Σ z 1 × 1 1\times1 1 × 1
2.3 “观测误差”到底指什么
工程代码中，观测误差可能指两件不同的事：传感器自身的物理测量误差，或者实际测量和预测测量之间的差。本文把后者称为观测残差，也称 innovation：
r ≜ z − h ( X ^ − ) . r\triangleq z-h(\hat{\mathcal X}^-). r ≜ z − h ( X ^ − ) . 这里 X ^ − \hat{\mathcal X}^- X ^ − n n n
这一区分很有用。滤波器并不直接问“这次传感器误差是多少”，因为真实状态未知；它能计算的是“实际读数和当前状态预测出的读数相差多少”。
3. 一般观测模型如何得到 H H H
3.1 从真实状态到预测状态
非线性观测模型为
z = h ( X ) + n . z=h(\mathcal X)+n. z = h ( X ) + n . 实际滤波器手里只有预测名义状态 X ^ − \hat{\mathcal X}^- X ^ −
z ^ = h ( X ^ − ) . \hat z=h(\hat{\mathcal X}^-). z ^ = h ( X ^ − ) . 真实状态写成预测名义状态加上误差：
X = X ^ − ⊞ δ x . \mathcal X=\hat{\mathcal X}^-\boxplus\delta x. X = X ^ − ⊞ δ x . 于是实际测量可以写成
z = h ( X ^ − ⊞ δ x ) + n . z=h(\hat{\mathcal X}^-\boxplus\delta x)+n. z = h ( X ^ − ⊞ δ x ) + n . 残差为
r = z − h ( X ^ − ) = h ( X ^ − ⊞ δ x ) − h ( X ^ − ) + n . \begin{aligned}
r
&=z-h(\hat{\mathcal X}^-)\\
&=h(\hat{\mathcal X}^-\boxplus\delta x)-h(\hat{\mathcal X}^-)+n.
\end{aligned} r = z − h ( X ^ − ) = h ( X ^ − ⊞ δ x ) − h ( X ^ − ) + n . 3.2 一阶展开
在 δ x = 0 \delta x=0 δ x = 0
h ( X ^ − ⊞ δ x ) ≈ h ( X ^ − ) + H δ x . h(\hat{\mathcal X}^-\boxplus\delta x)
\approx
h(\hat{\mathcal X}^-)+H\delta x. h ( X ^ − ⊞ δ x ) ≈ h ( X ^ − ) + H δ x . 其中
H ≜ ∂ h ( X ^ − ⊞ δ x ) ∂ δ x ∣ δ x = 0 \boxed{
H\triangleq
\left.
\frac{\partial h(\hat{\mathcal X}^-\boxplus\delta x)}
{\partial\delta x}
\right|_{\delta x=0}
} H ≜ ∂ δ x ∂ h ( X ^ − ⊞ δ x ) δ x = 0 就是观测模型对误差状态的 Jacobian。代回残差：
r ≈ H δ x + n . \boxed{
r\approx H\delta x+n.} r ≈ H δ x + n . 这就是 ESKF 观测更新使用的线性观测方程。
这里有一个经常被忽略的细节：H H H δ x \delta x δ x R R R p p p v v v b g b_g b g b a b_a b a R = R ^ Exp ( [ δ θ ] × ) R=\hat R\operatorname{Exp}([\delta\theta]_\times) R = R ^ Exp ([ δ θ ] × )
3.3 残差方向和 H H H
本文采用
r = z − z ^ . r=z-\hat z. r = z − z ^ . 如果你改用相反的残差
r ′ = z ^ − z = − r , r'=\hat z-z=-r, r ′ = z ^ − z = − r , 那么线性模型必须同时改成
r ′ ≈ − H δ x − n . r'\approx -H\delta x-n. r ′ ≈ − H δ x − n . 这时如果把残差取反，却继续使用原来的 H H H H H H
3.4 从 H H H
有了残差模型
r ≈ H δ x + n , r\approx H\delta x+n, r ≈ H δ x + n , 以及预测误差协方差 P − P^- P −
S = H P − H ⊤ + Σ z . S=HP^-H^\top+\Sigma_z. S = H P − H ⊤ + Σ z . Kalman gain 为
K = P − H ⊤ S − 1 . K=P^-H^\top S^{-1}. K = P − H ⊤ S − 1 . 滤波器更新的是局部误差状态均值：
δ x ^ = K r . \delta\hat x=Kr. δ x ^ = K r . 可以把这三步理解为：H H H S S S K K K
此时不要把 δ x ^ \delta\hat x δ x ^
3.5 从概率模型推导 Kalman 更新
前面的公式说明了更新怎么写，下面解释这些公式为什么是这个形式。推导只使用一个线性高斯模型，因此不依赖 ESKF 的具体传感器。ESKF 的作用是先把非线性观测在误差状态附近变成这个模型。
3.5.1 先写出先验和观测的概率模型
在当前观测到来之前，ESKF 已经通过 IMU 传播得到了预测误差协方差 P − P^- P −
δ x ∼ N ( 0 , P − ) . \delta x\sim\mathcal N(0,P^-). δ x ∼ N ( 0 , P − ) . 刚才得到的线性观测模型是
r = H δ x + n , n ∼ N ( 0 , Σ z ) . r=H\delta x+n,
\qquad
n\sim\mathcal N(0,\Sigma_z). r = H δ x + n , n ∼ N ( 0 , Σ z ) . 给定某个候选误差 δ x \delta x δ x
r ∣ δ x ∼ N ( H δ x , Σ z ) . r\mid\delta x
\sim
\mathcal N(H\delta x,\Sigma_z). r ∣ δ x ∼ N ( H δ x , Σ z ) . 滤波器要做的事情可以表述为：看到残差 r r r p ( δ x ∣ r ) p(\delta x\mid r) p ( δ x ∣ r )
3.5.2 由 Bayes 公式得到代价函数
Bayes 公式给出
p ( δ x ∣ r ) ∝ p ( r ∣ δ x ) p ( δ x ) . p(\delta x\mid r)
\propto
p(r\mid\delta x)p(\delta x). p ( δ x ∣ r ) ∝ p ( r ∣ δ x ) p ( δ x ) . 把两个高斯分布的指数部分代入，负对数后验等价于最小化下面的代价函数：
J ( δ x ) = δ x ⊤ ( P − ) − 1 δ x + ( r − H δ x ) ⊤ Σ z − 1 ( r − H δ x ) . \begin{aligned}
J(\delta x)= {}&
\delta x^\top(P^-)^{-1}\delta x\\
&+(r-H\delta x)^\top
\Sigma_z^{-1}(r-H\delta x).
\end{aligned} J ( δ x ) = δ x ⊤ ( P − ) − 1 δ x + ( r − H δ x ) ⊤ Σ z − 1 ( r − H δ x ) . 这里采用的概率假设是：预测误差 δ x \delta x δ x n n n
δ x ∼ N ( 0 , P − ) , n ∼ N ( 0 , Σ z ) , Cov ( δ x , n ) = 0. \delta x\sim\mathcal N(0,P^-),
\qquad
n\sim\mathcal N(0,\Sigma_z),
\qquad
\operatorname{Cov}(\delta x,n)=0. δ x ∼ N ( 0 , P − ) , n ∼ N ( 0 , Σ z ) , Cov ( δ x , n ) = 0. 第一项来自先验。它惩罚误差状态偏离 IMU 传播给出的零均值预测；P − P^- P − Σ z \Sigma_z Σ z
这就是一个带先验的加权最小二乘问题。Kalman Filter 可以看成在线递推地求解这类问题，并把历史数据压缩在当前的均值和协方差中。
3.5.3 对代价函数求导
先把第二项展开：
( r − H δ x ) ⊤ Σ z − 1 ( r − H δ x ) = r ⊤ Σ z − 1 r − r ⊤ Σ z − 1 H δ x − δ x ⊤ H ⊤ Σ z − 1 r + δ x ⊤ H ⊤ Σ z − 1 H δ x . \begin{aligned}
&(r-H\delta x)^\top\Sigma_z^{-1}(r-H\delta x)\\
={}&r^\top\Sigma_z^{-1}r
-r^\top\Sigma_z^{-1}H\delta x\\
&-\delta x^\top H^\top\Sigma_z^{-1}r
+\delta x^\top H^\top\Sigma_z^{-1}H\delta x.
\end{aligned} = ( r − H δ x ) ⊤ Σ z − 1 ( r − H δ x ) r ⊤ Σ z − 1 r − r ⊤ Σ z − 1 H δ x − δ x ⊤ H ⊤ Σ z − 1 r + δ x ⊤ H ⊤ Σ z − 1 H δ x . 因为 Σ z − 1 \Sigma_z^{-1} Σ z − 1
J ( δ x ) = δ x ⊤ ( ( P − ) − 1 + H ⊤ Σ z − 1 H ) δ x − 2 δ x ⊤ H ⊤ Σ z − 1 r + r ⊤ Σ z − 1 r . \begin{aligned}
J(\delta x)= {}&
\delta x^\top
\left((P^-)^{-1}+H^\top\Sigma_z^{-1}H\right)
\delta x\\
&-2\delta x^\top H^\top\Sigma_z^{-1}r
+r^\top\Sigma_z^{-1}r.
\end{aligned} J ( δ x ) = δ x ⊤ ( ( P − ) − 1 + H ⊤ Σ z − 1 H ) δ x − 2 δ x ⊤ H ⊤ Σ z − 1 r + r ⊤ Σ z − 1 r . 对 δ x \delta x δ x
∂ J ∂ δ x = 2 ( ( P − ) − 1 + H ⊤ Σ z − 1 H ) δ x − 2 H ⊤ Σ z − 1 r . \begin{aligned}
\frac{\partial J}{\partial\delta x}
={}&2\left((P^-)^{-1}+H^\top\Sigma_z^{-1}H\right)\delta x\\
&-2H^\top\Sigma_z^{-1}r.
\end{aligned} ∂ δ x ∂ J = 2 ( ( P − ) − 1 + H ⊤ Σ z − 1 H ) δ x − 2 H ⊤ Σ z − 1 r . 令梯度为零，得到正规方程：
( ( P − ) − 1 + H ⊤ Σ z − 1 H ) δ x ^ = H ⊤ Σ z − 1 r . \left((P^-)^{-1}+H^\top\Sigma_z^{-1}H\right)
\delta\hat x
=H^\top\Sigma_z^{-1}r. ( ( P − ) − 1 + H ⊤ Σ z − 1 H ) δ x ^ = H ⊤ Σ z − 1 r . 于是误差状态的后验均值首先可以写成信息形式：
δ x ^ = ( ( P − ) − 1 + H ⊤ Σ z − 1 H ) − 1 H ⊤ Σ z − 1 r . \boxed{
\delta\hat x
=\left((P^-)^{-1}+H^\top\Sigma_z^{-1}H\right)^{-1}
H^\top\Sigma_z^{-1}r.
} δ x ^ = ( ( P − ) − 1 + H ⊤ Σ z − 1 H ) − 1 H ⊤ Σ z − 1 r . 这个形式很能说明信息是如何累加的：先验信息矩阵是 ( P − ) − 1 (P^-)^{-1} ( P − ) − 1 H ⊤ Σ z − 1 H H^\top\Sigma_z^{-1}H H ⊤ Σ z − 1 H
3.5.4 把正规方程改写成 Kalman gain 形式
直接定义 Kalman gain
为简化记号，令
A ≜ ( P − ) − 1 + H ⊤ Σ z − 1 H . A\triangleq (P^-)^{-1}+H^\top\Sigma_z^{-1}H. A ≜ ( P − ) − 1 + H ⊤ Σ z − 1 H . 信息形式已经写成
δ x ^ = A − 1 H ⊤ Σ z − 1 r . \delta\hat x
=A^{-1}H^\top\Sigma_z^{-1}r. δ x ^ = A − 1 H ⊤ Σ z − 1 r . 观察这个式子：右边是一个矩阵乘以残差 r r r
K ≜ A − 1 H ⊤ Σ z − 1 . \boxed{
K\triangleq A^{-1}H^\top\Sigma_z^{-1}.
} K ≜ A − 1 H ⊤ Σ z − 1 . 于是更新公式马上变成
δ x ^ = K r . \boxed{
\delta\hat x=Kr.
} δ x ^ = K r . 因此，K K K
利用求逆引理得到观测空间形式
前面已经定义了信息形式下的 Kalman gain：
K ≜ A − 1 H ⊤ Σ z − 1 , A = ( P − ) − 1 + H ⊤ Σ z − 1 H . K\triangleq A^{-1}H^\top\Sigma_z^{-1},
\qquad
A=(P^-)^{-1}+H^\top\Sigma_z^{-1}H. K ≜ A − 1 H ⊤ Σ z − 1 , A = ( P − ) − 1 + H ⊤ Σ z − 1 H . 这个定义直接对应
δ x ^ = K r . \delta\hat x=Kr. δ x ^ = K r . 为了得到更适合工程计算的形式，使用矩阵求逆引理（Woodbury identity）的下面这个形式：
( Q − 1 + C ⊤ R − 1 C ) − 1 C ⊤ R − 1 = Q C ⊤ ( C Q C ⊤ + R ) − 1 . \boxed{
\left(Q^{-1}+C^\top R^{-1}C\right)^{-1}C^\top R^{-1}
=QC^\top\left(CQC^\top+R\right)^{-1}.
} ( Q − 1 + C ⊤ R − 1 C ) − 1 C ⊤ R − 1 = Q C ⊤ ( C Q C ⊤ + R ) − 1 . 在当前问题中令
Q = P − , C = H , R = Σ z . Q=P^-,
\qquad
C=H,
\qquad
R=\Sigma_z. Q = P − , C = H , R = Σ z . 那么求逆引理的左侧正好是信息形式中的 K K K
( Q − 1 + C ⊤ R − 1 C ) − 1 C ⊤ R − 1 = ( ( P − ) − 1 + H ⊤ Σ z − 1 H ) − 1 H ⊤ Σ z − 1 = A − 1 H ⊤ Σ z − 1 = K . \begin{aligned}
\left(Q^{-1}+C^\top R^{-1}C\right)^{-1}C^\top R^{-1}
&=\left((P^-)^{-1}+H^\top\Sigma_z^{-1}H\right)^{-1}
H^\top\Sigma_z^{-1}\\
&=A^{-1}H^\top\Sigma_z^{-1}\\
&=K.
\end{aligned} ( Q − 1 + C ⊤ R − 1 C ) − 1 C ⊤ R − 1 = ( ( P − ) − 1 + H ⊤ Σ z − 1 H ) − 1 H ⊤ Σ z − 1 = A − 1 H ⊤ Σ z − 1 = K . 求逆引理的右侧则变为
Q C ⊤ ( C Q C ⊤ + R ) − 1 = P − H ⊤ ( H P − H ⊤ + Σ z ) − 1 . QC^\top\left(CQC^\top+R\right)^{-1}
=P^-H^\top\left(HP^-H^\top+\Sigma_z\right)^{-1}. Q C ⊤ ( C Q C ⊤ + R ) − 1 = P − H ⊤ ( H P − H ⊤ + Σ z ) − 1 . 因此，K K K
K = P − H ⊤ ( H P − H ⊤ + Σ z ) − 1 . \boxed{
K
=P^-H^\top\left(HP^-H^\top+\Sigma_z\right)^{-1}.
} K = P − H ⊤ ( H P − H ⊤ + Σ z ) − 1 . 记残差协方差为
S ≜ H P − H ⊤ + Σ z , S\triangleq HP^-H^\top+\Sigma_z, S ≜ H P − H ⊤ + Σ z , 则上式就是熟悉的 Kalman gain 形式：
K = P − H ⊤ S − 1 . \boxed{
K=P^-H^\top S^{-1}.
} K = P − H ⊤ S − 1 . 这里的关键是：求逆引理没有改变 K K K 15 × 15 15\times15 15 × 15 A A A m × m m\times m m × m S S S m = 2 m=2 m = 2 m = 1 m=1 m = 1 S − 1 S^{-1} S − 1 P − H ⊤ S − 1 P^-H^\top S^{-1} P − H ⊤ S − 1
求逆引理的一个证明
为了证明上面的求逆引理，记
L ≜ Q − 1 + C ⊤ R − 1 C , M ≜ C Q C ⊤ + R , L\triangleq Q^{-1}+C^\top R^{-1}C,
\qquad
M\triangleq CQC^\top+R, L ≜ Q − 1 + C ⊤ R − 1 C , M ≜ C Q C ⊤ + R , 并定义
X ≜ Q C ⊤ M − 1 . X\triangleq QC^\top M^{-1}. X ≜ Q C ⊤ M − 1 . 将 X X X L L L
L X = ( Q − 1 + C ⊤ R − 1 C ) Q C ⊤ M − 1 = ( C ⊤ + C ⊤ R − 1 C Q C ⊤ ) M − 1 = C ⊤ ( I + R − 1 C Q C ⊤ ) M − 1 = C ⊤ R − 1 ( R + C Q C ⊤ ) M − 1 = C ⊤ R − 1 M M − 1 = C ⊤ R − 1 . \begin{aligned}
LX
&=\left(Q^{-1}+C^\top R^{-1}C\right)
QC^\top M^{-1}\\
&=\left(C^\top+C^\top R^{-1}CQC^\top\right)M^{-1}\\
&=C^\top\left(I+R^{-1}CQC^\top\right)M^{-1}\\
&=C^\top R^{-1}\left(R+CQC^\top\right)M^{-1}\\
&=C^\top R^{-1}MM^{-1}\\
&=C^\top R^{-1}.
\end{aligned} L X = ( Q − 1 + C ⊤ R − 1 C ) Q C ⊤ M − 1 = ( C ⊤ + C ⊤ R − 1 C Q C ⊤ ) M − 1 = C ⊤ ( I + R − 1 C Q C ⊤ ) M − 1 = C ⊤ R − 1 ( R + C Q C ⊤ ) M − 1 = C ⊤ R − 1 M M − 1 = C ⊤ R − 1 . 由于 L L L L − 1 L^{-1} L − 1
X = L − 1 C ⊤ R − 1 . X=L^{-1}C^\top R^{-1}. X = L − 1 C ⊤ R − 1 . 再代回 L L L M M M X X X
Q C ⊤ ( C Q C ⊤ + R ) − 1 = ( Q − 1 + C ⊤ R − 1 C ) − 1 C ⊤ R − 1 . QC^\top\left(CQC^\top+R\right)^{-1}
=\left(Q^{-1}+C^\top R^{-1}C\right)^{-1}C^\top R^{-1}. Q C ⊤ ( C Q C ⊤ + R ) − 1 = ( Q − 1 + C ⊤ R − 1 C ) − 1 C ⊤ R − 1 . 这就证明了前面使用的矩阵求逆引理。
最终，误差状态更新仍然是
δ x ^ = K r . \delta\hat x=Kr. δ x ^ = K r . 信息形式适合从概率模型推导更新，观测空间形式适合实际计算；二者通过求逆引理完全等价。
3.5.5 S S S
Kalman gain 中的矩阵
S = H P − H ⊤ + Σ z S=HP^-H^\top+\Sigma_z S = H P − H ⊤ + Σ z 也可以直接从残差的随机变量表达式看出来：
r = H δ x + n . r=H\delta x+n. r = H δ x + n . 由于先验误差和观测噪声相互独立，且二者均值为零，
Cov ( r ) = Cov ( H δ x + n ) = H Cov ( δ x ) H ⊤ + Cov ( n ) = H P − H ⊤ + Σ z . \begin{aligned}
\operatorname{Cov}(r)
&=\operatorname{Cov}(H\delta x+n)\\
&=H\operatorname{Cov}(\delta x)H^\top
+\operatorname{Cov}(n)\\
&=HP^-H^\top+\Sigma_z.
\end{aligned} Cov ( r ) = Cov ( H δ x + n ) = H Cov ( δ x ) H ⊤ + Cov ( n ) = H P − H ⊤ + Σ z . 所以 S S S
如果 S S S S S S
3.5.6 后验协方差从哪里来
代价函数的二次项矩阵就是注入前的后验信息矩阵：
( P ~ + ) − 1 = ( P − ) − 1 + H ⊤ Σ z − 1 H . (\widetilde P^+)^{-1}
=(P^-)^{-1}+H^\top\Sigma_z^{-1}H. ( P + ) − 1 = ( P − ) − 1 + H ⊤ Σ z − 1 H . 因此，误差注入前的后验协方差在信息形式下为
P ~ + = ( ( P − ) − 1 + H ⊤ Σ z − 1 H ) − 1 . \widetilde P^+
=\left((P^-)^{-1}
+H^\top\Sigma_z^{-1}H\right)^{-1}. P + = ( ( P − ) − 1 + H ⊤ Σ z − 1 H ) − 1 . 利用和上面相同的矩阵恒等式，可以把它改写成 Kalman 形式：
P ~ + = P − − P − H ⊤ S − 1 H P − . \boxed{
\widetilde P^+
=P^--P^-H^\top S^{-1}HP^-.
} P + = P − − P − H ⊤ S − 1 H P − . 这里用 P ~ + \widetilde P^+ P +
还可以从更新后的估计误差直接验证这个结果。定义
e + = δ x − δ x ^ . e^+=\delta x-\delta\hat x. e + = δ x − δ x ^ . 由于 δ x ^ = K r \delta\hat x=Kr δ x ^ = K r r = H δ x + n r=H\delta x+n r = H δ x + n
e + = ( I − K H ) δ x − K n . e^+=(I-KH)\delta x-Kn. e + = ( I − K H ) δ x − K n . 因此
P ~ + = ( I − K H ) P − ( I − K H ) ⊤ + K Σ z K ⊤ . \boxed{
\widetilde P^+
=(I-KH)P^-(I-KH)^\top
+K\Sigma_zK^\top.
} P + = ( I − K H ) P − ( I − K H ) ⊤ + K Σ z K ⊤ . 这就是 Joseph 形式。现在把
S = H P − H ⊤ + Σ z , K = P − H ⊤ S − 1 S=HP^-H^\top+\Sigma_z,
\qquad
K=P^-H^\top S^{-1} S = H P − H ⊤ + Σ z , K = P − H ⊤ S − 1 代入并逐项展开：
P ~ + = ( I − K H ) P − ( I − K H ) ⊤ + K Σ z K ⊤ = P − − K H P − − P − H ⊤ K ⊤ + K ( H P − H ⊤ + Σ z ) K ⊤ = P − − K H P − − P − H ⊤ K ⊤ + K S K ⊤ . \begin{aligned}
\widetilde P^+
&=(I-KH)P^-(I-KH)^\top+K\Sigma_zK^\top\\
&=P^- -KHP^- -P^-H^\top K^\top
+K\left(HP^-H^\top+\Sigma_z\right)K^\top\\
&=P^- -KHP^- -P^-H^\top K^\top+KSK^\top.
\end{aligned} P + = ( I − K H ) P − ( I − K H ) ⊤ + K Σ z K ⊤ = P − − K H P − − P − H ⊤ K ⊤ + K ( H P − H ⊤ + Σ z ) K ⊤ = P − − K H P − − P − H ⊤ K ⊤ + K S K ⊤ . 关键的一步是利用 Kalman gain 的定义：
K S = P − H ⊤ S − 1 S = P − H ⊤ . KS
=P^-H^\top S^{-1}S
=P^-H^\top. K S = P − H ⊤ S − 1 S = P − H ⊤ . 因此
K S K ⊤ = P − H ⊤ K ⊤ , KSK^\top=P^-H^\top K^\top, K S K ⊤ = P − H ⊤ K ⊤ , 上式中的两项正好抵消：
P ~ + = P − − K H P − = P − − P − H ⊤ S − 1 H P − . \begin{aligned}
\widetilde P^+
&=P^- -KHP^-\\
&=P^- -P^-H^\top S^{-1}HP^-.
\end{aligned} P + = P − − K H P − = P − − P − H ⊤ S − 1 H P − . 所以，下面三种写法在标准 Kalman gain 条件下是同一个后验协方差：
P ~ + = ( I − K H ) P − = P − − P − H ⊤ S − 1 H P − = ( I − K H ) P − ( I − K H ) ⊤ + K Σ z K ⊤ . \boxed{
\begin{aligned}
\widetilde P^+
&=(I-KH)P^-\\
&=P^- -P^-H^\top S^{-1}HP^-\\
&=(I-KH)P^-(I-KH)^\top+K\Sigma_zK^\top.
\end{aligned}
} P + = ( I − K H ) P − = P − − P − H ⊤ S − 1 H P − = ( I − K H ) P − ( I − K H ) ⊤ + K Σ z K ⊤ . 这里的等价关系依赖于
K = P − H ⊤ S − 1 , S = H P − H ⊤ + Σ z . K=P^-H^\top S^{-1},
\qquad
S=HP^-H^\top+\Sigma_z. K = P − H ⊤ S − 1 , S = H P − H ⊤ + Σ z . 如果 K K K ( I − K H ) P − (I-KH)P^- ( I − K H ) P − P − P^- P − Σ z \Sigma_z Σ z S S S
在数学上这些形式等价；在浮点计算中，Joseph 形式通常更适合保持协方差的对称性和半正定性。因此实际实现常用
P ~ + = ( I − K H ) P − ( I − K H ) ⊤ + K Σ z K ⊤ \widetilde P^+
=(I-KH)P^-(I-KH)^\top
+K\Sigma_zK^\top P + = ( I − K H ) P − ( I − K H ) ⊤ + K Σ z K ⊤ 而不是直接使用化简后的表达式。
3.5.7 用一维例子理解 Kalman gain
把所有矩阵暂时换成标量。设先验误差方差为 p p p
r = δ x + n , Var ( n ) = σ 2 . r=\delta x+n,
\qquad
\operatorname{Var}(n)=\sigma^2. r = δ x + n , Var ( n ) = σ 2 . 此时
K = p p + σ 2 , δ x ^ = K r . K=\frac{p}{p+\sigma^2},
\qquad
\delta\hat x=Kr. K = p + σ 2 p , δ x ^ = K r . 如果先验很不确定，p ≫ σ 2 p\gg\sigma^2 p ≫ σ 2
K ≈ 1 , K\approx1, K ≈ 1 , 滤波器更相信观测残差；如果传感器噪声很大，σ 2 ≫ p \sigma^2\gg p σ 2 ≫ p
K ≈ 0 , K\approx0, K ≈ 0 , 滤波器主要保留 IMU 预测。
多维情况下，P − H ⊤ P^-H^\top P − H ⊤ S − 1 S^{-1} S − 1
4. 位置观测：最简单的 H H H
4.1 观测模型
假设外部传感器直接给出 IMU 原点在世界系中的位置：
z p = p W + n p . z_p=p_W+n_p. z p = p W + n p . 这是一个三维观测，z p ∈ R 3 z_p\in\mathbb R^3 z p ∈ R 3
z ^ p = p ^ W . \hat z_p=\hat p_W. z ^ p = p ^ W . 残差为
r p = z p − p ^ W . r_p=z_p-\hat p_W. r p = z p − p ^ W . 4.2 代入误差状态
真实位置为
p W = p ^ W + δ p . p_W=\hat p_W+\delta p. p W = p ^ W + δ p . 所以
z p = p ^ W + δ p + n p , r p = z p − p ^ W ≈ δ p + n p . \begin{aligned}
z_p
&=\hat p_W+\delta p+n_p,\\
r_p
&=z_p-\hat p_W\\
&\approx\delta p+n_p.
\end{aligned} z p r p = p ^ W + δ p + n p , = z p − p ^ W ≈ δ p + n p . 因此
H p = [ 0 I 0 0 0 ] . \boxed{
H_p=
\begin{bmatrix}
0&I&0&0&0
\end{bmatrix}.
} H p = [ 0 I 0 0 0 ] . 这里的 0 0 0 I I I 3 × 3 3\times3 3 × 3 H p H_p H p 3 × 15 3\times15 3 × 15
这个结果很直观：位置观测对位置误差敏感，对姿态、速度和两个 bias 没有直接的一阶敏感性。
4.3 “没有直接敏感”不等于“永远不影响”
H p H_p H p P − P^- P −
P p θ − ≠ 0 , P p b a − ≠ 0. P^-_{p\theta}\neq0,
\qquad
P^-_{p b_a}\neq0. P pθ − = 0 , P p b a − = 0. Kalman gain 使用的是
K = P − H p ⊤ S − 1 . K=P^-H_p^\top S^{-1}. K = P − H p ⊤ S − 1 . 即使 H p H_p H p P − H p ⊤ P^-H_p^\top P − H p ⊤
这也是读 H H H H H H K K K
5. 速度观测：把 p p p v v v
5.1 观测模型和残差
如果轮速计或外部速度估计器给出世界系速度观测：
z v = v W + n v , z_v=v_W+n_v, z v = v W + n v , 则
z ^ v = v ^ W , r v = z v − v ^ W . \hat z_v=\hat v_W,
\qquad
r_v=z_v-\hat v_W. z ^ v = v ^ W , r v = z v − v ^ W . 代入
v W = v ^ W + δ v , v_W=\hat v_W+\delta v, v W = v ^ W + δ v , 得到
r v ≈ δ v + n v . r_v\approx\delta v+n_v. r v ≈ δ v + n v . 所以
H v = [ 0 0 I 0 0 ] . \boxed{
H_v=
\begin{bmatrix}
0&0&I&0&0
\end{bmatrix}.
} H v = [ 0 0 I 0 0 ] . 同样，H v H_v H v 3 × 15 3\times15 3 × 15
5.2 速度坐标系必须先说清楚
上面的模型假设观测是世界系速度。如果轮速计给出的是机体系前向速度，观测模型就不是 z v = v W + n v z_v=v_W+n_v z v = v W + n v
例如，假设传感器测量机体系中的速度：
z v B = R W B ⊤ v W + n v B . z_{v_B}=R_{WB}^{\top}v_W+n_{v_B}. z v B = R W B ⊤ v W + n v B . 预测值为
z ^ v B = R ^ W B ⊤ v ^ W . \hat z_{v_B}=\hat R_{WB}^{\top}\hat v_W. z ^ v B = R ^ W B ⊤ v ^ W . 把真实状态代入：
R W B ⊤ v W = Exp ( − [ δ θ ] × ) R ^ W B ⊤ ( v ^ W + δ v ) ≈ ( I − [ δ θ ] × ) ( v ^ B + R ^ W B ⊤ δ v ) ≈ v ^ B − [ δ θ ] × v ^ B + R ^ W B ⊤ δ v , \begin{aligned}
R_{WB}^{\top}v_W
&=\operatorname{Exp}(-[\delta\theta]_\times)
\hat R_{WB}^{\top}(\hat v_W+\delta v)\\
&\approx
( I-[\delta\theta]_\times )
(\hat v_B+\hat R_{WB}^{\top}\delta v)\\
&\approx
\hat v_B-[\delta\theta]_\times\hat v_B
+\hat R_{WB}^{\top}\delta v,
\end{aligned} R W B ⊤ v W = Exp ( − [ δ θ ] × ) R ^ W B ⊤ ( v ^ W + δ v ) ≈ ( I − [ δ θ ] × ) ( v ^ B + R ^ W B ⊤ δ v ) ≈ v ^ B − [ δ θ ] × v ^ B + R ^ W B ⊤ δ v , 其中
v ^ B ≜ R ^ W B ⊤ v ^ W . \hat v_B\triangleq\hat R_{WB}^{\top}\hat v_W. v ^ B ≜ R ^ W B ⊤ v ^ W . 利用
− [ δ θ ] × v ^ B = [ v ^ B ] × δ θ , -[\delta\theta]_\times\hat v_B
=[\hat v_B]_\times\delta\theta, − [ δ θ ] × v ^ B = [ v ^ B ] × δ θ , 可得
r v B ≈ [ v ^ B ] × δ θ + R ^ W B ⊤ δ v + n v B . r_{v_B}
\approx
[\hat v_B]_\times\delta\theta
+\hat R_{WB}^{\top}\delta v+n_{v_B}. r v B ≈ [ v ^ B ] × δ θ + R ^ W B ⊤ δ v + n v B . 所以机体系速度观测的 Jacobian 是
H v B = [ [ v ^ B ] × 0 R ^ W B ⊤ 0 0 ] . \boxed{
H_{v_B}=
\begin{bmatrix}
[\hat v_B]_\times&0&\hat R_{WB}^{\top}&0&0
\end{bmatrix}.
} H v B = [ [ v ^ B ] × 0 R ^ W B ⊤ 0 0 ] . 同一个“速度观测”，只因为输出坐标系不同，H H H
6. 姿态观测：不能用矩阵相减
6.1 一个适合旋转的观测模型
假设视觉、磁力计或其他外部模块给出姿态观测 z R ∈ S O ( 3 ) z_R\in SO(3) z R ∈ S O ( 3 )
z R = R W B Exp ( [ η R ] × ) , z_R=R_{WB}\operatorname{Exp}([\eta_R]_\times), z R = R W B Exp ([ η R ] × ) , 其中 η R ∈ R 3 \eta_R\in\mathbb R^3 η R ∈ R 3
旋转没有普通向量意义下的减法，因此残差不能写成
z R − R ^ W B . z_R-\hat R_{WB}. z R − R ^ W B . 可以把两者相乘得到相对旋转，再取对数映射：
ρ R ≜ Log ( R ^ W B ⊤ z R ) ∨ . \rho_R
\triangleq
\operatorname{Log}\left(\hat R_{WB}^{\top}z_R\right)^\vee. ρ R ≜ Log ( R ^ W B ⊤ z R ) ∨ . 这里 ∨ \vee ∨
6.2 代入右侧姿态误差
由
R W B = R ^ W B Exp ( [ δ θ ] × ) R_{WB}=\hat R_{WB}\operatorname{Exp}([\delta\theta]_\times) R W B = R ^ W B Exp ([ δ θ ] × ) 和观测模型，得到
R ^ W B ⊤ z R = R ^ W B ⊤ R W B Exp ( [ η R ] × ) = Exp ( [ δ θ ] × ) Exp ( [ η R ] × ) . \begin{aligned}
\hat R_{WB}^{\top}z_R
&=\hat R_{WB}^{\top}R_{WB}
\operatorname{Exp}([\eta_R]_\times)\\
&=\operatorname{Exp}([\delta\theta]_\times)
\operatorname{Exp}([\eta_R]_\times).
\end{aligned} R ^ W B ⊤ z R = R ^ W B ⊤ R W B Exp ([ η R ] × ) = Exp ([ δ θ ] × ) Exp ([ η R ] × ) . 在两个量都足够小时，Baker-Campbell-Hausdorff 展开的一阶结果是
Log ( Exp ( [ δ θ ] × ) Exp ( [ η R ] × ) ) ∨ ≈ δ θ + η R . \operatorname{Log}
\left(\operatorname{Exp}([\delta\theta]_\times)
\operatorname{Exp}([\eta_R]_\times)\right)^\vee
\approx\delta\theta+\eta_R. Log ( Exp ([ δ θ ] × ) Exp ([ η R ] × ) ) ∨ ≈ δ θ + η R . 因此
ρ R ≈ δ θ + η R . \rho_R\approx\delta\theta+\eta_R. ρ R ≈ δ θ + η R . 姿态观测的 Jacobian 为
H R = [ I 0 0 0 0 ] . \boxed{
H_R=
\begin{bmatrix}
I&0&0&0&0
\end{bmatrix}.
} H R = [ I 0 0 0 0 ] . 6.3 乘法顺序改变，结果也会改变
上面的简单结果依赖三个约定：右侧姿态误差、右侧观测噪声，以及残差
Log ( R ^ ⊤ z R ) ∨ . \operatorname{Log}(\hat R^{\top}z_R)^\vee. Log ( R ^ ⊤ z R ) ∨ . 如果观测噪声改放到左侧，或者残差改成
Log ( z R ⊤ R ^ ) ∨ , \operatorname{Log}(z_R^{\top}\hat R)^\vee, Log ( z R ⊤ R ^ ) ∨ , 误差向量的坐标系和符号都会变化。不能看到两个旋转矩阵，就直接把 H θ H_\theta H θ I I I
7. 重力观测：姿态误差 Jacobian 的一个典型来源
重力观测很适合练习右侧姿态扰动，因为它只观测一个世界系方向。设世界系重力加速度为 g W g_W g W
z g = R W B ⊤ g W + n g . z_g=R_{WB}^{\top}g_W+n_g. z g = R W B ⊤ g W + n g . 这里的 n g n_g n g
7.1 预测观测
定义名义姿态下的重力投影：
g ^ B ≜ R ^ W B ⊤ g W . \hat g_B\triangleq\hat R_{WB}^{\top}g_W. g ^ B ≜ R ^ W B ⊤ g W . 预测观测是
z ^ g = g ^ B . \hat z_g=\hat g_B. z ^ g = g ^ B . 7.2 展开真实重力投影
由右侧姿态误差
R W B ⊤ = Exp ( − [ δ θ ] × ) R ^ W B ⊤ , R_{WB}^{\top}
=\operatorname{Exp}(-[\delta\theta]_\times)
\hat R_{WB}^{\top}, R W B ⊤ = Exp ( − [ δ θ ] × ) R ^ W B ⊤ , 有
R W B ⊤ g W = Exp ( − [ δ θ ] × ) g ^ B ≈ ( I − [ δ θ ] × ) g ^ B = g ^ B − [ δ θ ] × g ^ B . \begin{aligned}
R_{WB}^{\top}g_W
&=\operatorname{Exp}(-[\delta\theta]_\times)\hat g_B\\
&\approx (I-[\delta\theta]_\times)\hat g_B\\
&=\hat g_B-[\delta\theta]_\times\hat g_B.
\end{aligned} R W B ⊤ g W = Exp ( − [ δ θ ] × ) g ^ B ≈ ( I − [ δ θ ] × ) g ^ B = g ^ B − [ δ θ ] × g ^ B . 利用叉乘交换关系：
− [ δ θ ] × g ^ B = [ g ^ B ] × δ θ , -[\delta\theta]_\times\hat g_B
=[\hat g_B]_\times\delta\theta, − [ δ θ ] × g ^ B = [ g ^ B ] × δ θ , 因此
R W B ⊤ g W ≈ g ^ B + [ g ^ B ] × δ θ . R_{WB}^{\top}g_W
\approx
\hat g_B+[\hat g_B]_\times\delta\theta. R W B ⊤ g W ≈ g ^ B + [ g ^ B ] × δ θ . 代回残差：
r g = z g − g ^ B ≈ [ g ^ B ] × δ θ + n g . \begin{aligned}
r_g
&=z_g-\hat g_B\\
&\approx[\hat g_B]_\times\delta\theta+n_g.
\end{aligned} r g = z g − g ^ B ≈ [ g ^ B ] × δ θ + n g . 所以
H g = [ [ g ^ B ] × 0 0 0 0 ] . \boxed{
H_g=
\begin{bmatrix}
[\hat g_B]_\times&0&0&0&0
\end{bmatrix}.
} H g = [ [ g ^ B ] × 0 0 0 0 ] . 这个 H g H_g H g 3 × 15 3\times15 3 × 15 [ g ^ B ] × [\hat g_B]_\times [ g ^ B ] × 2 2 2
[ g ^ B ] × g ^ B = 0. [\hat g_B]_\times\hat g_B=0. [ g ^ B ] × g ^ B = 0. 这不是数值实现的缺陷，而是观测模型本身的几何退化。
7.3 静止加速度计不是同一个符号
上一篇文章使用的加速度计模型为
a ~ = R W B ⊤ ( a W − g W ) + b a + n a . \tilde a
=R_{WB}^{\top}(a_W-g_W)+b_a+n_a. a ~ = R W B ⊤ ( a W − g W ) + b a + n a . 静止时 a W = 0 a_W=0 a W = 0
z a = − R W B ⊤ g W + b a + n a . z_a=-R_{WB}^{\top}g_W+b_a+n_a. z a = − R W B ⊤ g W + b a + n a . 注意这里是 − R W B ⊤ g W -R_{WB}^{\top}g_W − R W B ⊤ g W
定义
a ^ g ≜ − R ^ W B ⊤ g W . \hat a_g\triangleq-\hat R_{WB}^{\top}g_W. a ^ g ≜ − R ^ W B ⊤ g W . 真实观测的一阶展开为
z a = − Exp ( − [ δ θ ] × ) R ^ W B ⊤ g W + b ^ a + δ b a + n a ≈ − R ^ W B ⊤ g W − [ g ^ B ] × δ θ + b ^ a + δ b a + n a . \begin{aligned}
z_a
&=-\operatorname{Exp}(-[\delta\theta]_\times)
\hat R_{WB}^{\top}g_W
+\hat b_a+\delta b_a+n_a\\
&\approx
-\hat R_{WB}^{\top}g_W
-[\hat g_B]_\times\delta\theta
+\hat b_a+\delta b_a+n_a.
\end{aligned} z a = − Exp ( − [ δ θ ] × ) R ^ W B ⊤ g W + b ^ a + δ b a + n a ≈ − R ^ W B ⊤ g W − [ g ^ B ] × δ θ + b ^ a + δ b a + n a . 因此
r a ≈ − [ g ^ B ] × δ θ + δ b a + n a , r_a
\approx
-[\hat g_B]_\times\delta\theta
+\delta b_a+n_a, r a ≈ − [ g ^ B ] × δ θ + δ b a + n a , 对应的 Jacobian 是
H a = [ − [ g ^ B ] × 0 0 0 I ] . \boxed{
H_a=
\begin{bmatrix}
-[\hat g_B]_\times&0&0&0&I
\end{bmatrix}.
} H a = [ − [ g ^ B ] × 0 0 0 I ] . 这两个矩阵很容易写反：纯重力向量观测的姿态 block 是 + [ g ^ B ] × +[\hat g_B]_\times + [ g ^ B ] × − [ g ^ B ] × -[\hat g_B]_\times − [ g ^ B ] × I I I
这也解释了为什么静止时不能仅凭加速度计均值同时精确得到姿态和加速度计 bias。两者都进入同一个观测通道，线性化后会产生耦合。
7.4 如果只使用重力方向
有些系统不使用加速度计的幅值，只使用归一化方向：
z ˉ a = z a ∥ z a ∥ . \bar z_a=\frac{z_a}{\lVert z_a\rVert}. z ˉ a = ∥ z a ∥ z a . 此时不能直接把未归一化观测的 H a H_a H a
ν ( x ) = x ∥ x ∥ \nu(x)=\frac{x}{\lVert x\rVert} ν ( x ) = ∥ x ∥ x 其 Jacobian 为
∂ ν ∂ x = 1 ∥ x ∥ ( I − ν ν ⊤ ) . \frac{\partial\nu}{\partial x}
=\frac{1}{\lVert x\rVert}
\left(I-\nu\nu^\top\right). ∂ x ∂ ν = ∥ x ∥ 1 ( I − ν ν ⊤ ) . 若 H a , r a w H_{a,\mathrm{raw}} H a , raw
H a , d i r = 1 ∥ a ^ g ∥ ( I − u ^ u ^ ⊤ ) H a , r a w , H_{a,\mathrm{dir}}
=\frac{1}{\lVert\hat a_g\rVert}
\left(I-\hat u\hat u^\top\right)H_{a,\mathrm{raw}}, H a , dir = ∥ a ^ g ∥ 1 ( I − u ^ u ^ ⊤ ) H a , raw , 其中
u ^ = a ^ g ∥ a ^ g ∥ . \hat u=\frac{\hat a_g}{\lVert\hat a_g\rVert}. u ^ = ∥ a ^ g ∥ a ^ g . 投影矩阵 I − u ^ u ^ ⊤ I-\hat u\hat u^\top I − u ^ u ^ ⊤
8. LiDAR 点到平面观测：把几何关系写成 H H H
前面的例子直接读取状态中的位置、速度或重力投影。LiDAR 点到平面约束更接近实际 LIO：观测不是状态的某一个分量，而是由姿态、位置和点坐标共同组成的几何量。
8.1 几何模型
设一个 LiDAR 点已经转换到 IMU 机体系，坐标为 q B q_B q B
q W = p W + R W B q B . q_W=p_W+R_{WB}q_B. q W = p W + R W B q B . 设该点对应的世界系平面满足
n W ⊤ x W = d , n_W^\top x_W=d, n W ⊤ x W = d , 其中 n W n_W n W d d d
理想情况下，点落在平面上：
n W ⊤ q W = d . n_W^\top q_W=d. n W ⊤ q W = d . 把它写成标量观测模型：
z π = d + n π , z_\pi=d+n_\pi, z π = d + n π , h π ( X ) = n W ⊤ ( p W + R W B q B ) . h_\pi(\mathcal X)=n_W^\top(p_W+R_{WB}q_B). h π ( X ) = n W ⊤ ( p W + R W B q B ) . 残差采用本文统一的“实际观测减预测观测”：
r π = z π − h π ( X ^ ) . r_\pi=z_\pi-h_\pi(\hat{\mathcal X}). r π = z π − h π ( X ^ ) . 如果实现中习惯使用点到平面的代数残差 h π − z π h_\pi-z_\pi h π − z π
8.2 位置部分
先展开位置：
p W = p ^ W + δ p . p_W=\hat p_W+\delta p. p W = p ^ W + δ p . 位置引起的预测观测变化为
n W ⊤ p W = n W ⊤ p ^ W + n W ⊤ δ p . n_W^\top p_W
=n_W^\top\hat p_W+n_W^\top\delta p. n W ⊤ p W = n W ⊤ p ^ W + n W ⊤ δ p . 因此位置 block 是 n W ⊤ n_W^\top n W ⊤
8.3 姿态部分
再处理旋转后的点。由
R W B = R ^ W B Exp ( [ δ θ ] × ) R_{WB}=\hat R_{WB}\operatorname{Exp}([\delta\theta]_\times) R W B = R ^ W B Exp ([ δ θ ] × ) 和一阶近似，
R W B q B ≈ R ^ W B ( I + [ δ θ ] × ) q B = R ^ W B q B + R ^ W B [ δ θ ] × q B . \begin{aligned}
R_{WB}q_B
&\approx\hat R_{WB}(I+[\delta\theta]_\times)q_B\\
&=\hat R_{WB}q_B+\hat R_{WB}[\delta\theta]_\times q_B.
\end{aligned} R W B q B ≈ R ^ W B ( I + [ δ θ ] × ) q B = R ^ W B q B + R ^ W B [ δ θ ] × q B . 利用
[ δ θ ] × q B = − [ q B ] × δ θ , [\delta\theta]_\times q_B
=-[q_B]_\times\delta\theta, [ δ θ ] × q B = − [ q B ] × δ θ , 得到
R W B q B ≈ R ^ W B q B − R ^ W B [ q B ] × δ θ . R_{WB}q_B
\approx
\hat R_{WB}q_B-\hat R_{WB}[q_B]_\times\delta\theta. R W B q B ≈ R ^ W B q B − R ^ W B [ q B ] × δ θ . 定义名义世界系点坐标
q ^ W ≜ R ^ W B q B . \hat q_W\triangleq\hat R_{WB}q_B. q ^ W ≜ R ^ W B q B . 则点到平面预测值的一阶展开为
h π ( X ) ≈ n W ⊤ ( p ^ W + q ^ W ) + n W ⊤ δ p − n W ⊤ R ^ W B [ q B ] × δ θ . \begin{aligned}
h_\pi(\mathcal X)
&\approx n_W^\top(\hat p_W+\hat q_W)\\
&\quad+n_W^\top\delta p
-n_W^\top\hat R_{WB}[q_B]_\times\delta\theta.
\end{aligned} h π ( X ) ≈ n W ⊤ ( p ^ W + q ^ W ) + n W ⊤ δ p − n W ⊤ R ^ W B [ q B ] × δ θ . 因此，按照 r = z − h ( X ^ ) r=z-h(\hat{\mathcal X}) r = z − h ( X ^ )
r π ≈ − n W ⊤ R ^ W B [ q B ] × δ θ + n W ⊤ δ p + n π . r_\pi
\approx
-n_W^\top\hat R_{WB}[q_B]_\times\delta\theta
+n_W^\top\delta p+n_\pi. r π ≈ − n W ⊤ R ^ W B [ q B ] × δ θ + n W ⊤ δ p + n π . 对应的 1 × 15 1\times15 1 × 15
H π = [ − n W ⊤ R ^ W B [ q B ] × n W ⊤ 0 0 0 ] . \boxed{
H_\pi=
\begin{bmatrix}
-n_W^\top\hat R_{WB}[q_B]_\times
&n_W^\top
&0
&0
&0
\end{bmatrix}.
} H π = [ − n W ⊤ R ^ W B [ q B ] × n W ⊤ 0 0 0 ] . 8.4 这个 Jacobian 的几何意义
姿态 block
− n W ⊤ R ^ W B [ q B ] × -n_W^\top\hat R_{WB}[q_B]_\times − n W ⊤ R ^ W B [ q B ] × 表示：机体绕一个小角度转动时，点的位置如何移动，以及这个移动在平面法向方向上有多少分量。
位置 block n W ⊤ n_W^\top n W ⊤ t W t_W t W n W ⊤ t W = 0 n_W^\top t_W=0 n W ⊤ t W = 0
H p t W = n W ⊤ t W = 0. H_p t_W=n_W^\top t_W=0. H p t W = n W ⊤ t W = 0. 这说明单个平面约束不能约束沿平面切向的平移。多个方向不同的平面，或者连续帧中的运动，才能逐渐消除这些退化方向。
8.5 外参不能被悄悄省略
上面把 q B q_B q B L L L
q B = t B L + R B L q L . q_B=t_{BL}+R_{BL}q_L. q B = t B L + R B L q L . 这里的具体下标含义必须由定义确认。若外参固定，q B q_B q B H H H
最容易出错的做法是：法向量用世界系、点用 LiDAR 系、旋转矩阵却按 IMU 到世界系套公式。每一项放入点到平面方程前，都必须已经在同一个坐标系中。
8.6 视觉重投影误差：从世界点到像素坐标
视觉观测是 ESKF 中最经典的一类观测。相机并不直接测量位置或姿态，而是测量一个世界点在图像上的像素位置。这个观测模型包含坐标变换和透视除法，正好可以把前面的姿态误差、位置误差和链式求导串起来。
下面先使用一个简化但完整的模型：地图点在世界系中已知，相机内参和 IMU-相机外参已知，状态中暂时不估计地图点和外参。最后再说明这些假设改变后 H H H
8.6.1 相机观测模型
设世界系中的地图点为 P W P_W P W C C C B B B C C C
p C = R C B p B + t C B , p_C=R_{CB}p_B+t_{CB}, p C = R C B p B + t C B , 其中 R C B R_{CB} R C B t C B t_{CB} t C B
先把世界点变换到 IMU 系。由于 R W B R_{WB} R W B
ℓ B = R W B ⊤ ( P W − p W ) . \ell_B=R_{WB}^{\top}(P_W-p_W). ℓ B = R W B ⊤ ( P W − p W ) . 再变换到相机系：
ℓ C = R C B ℓ B + t C B . \ell_C=R_{CB}\ell_B+t_{CB}. ℓ C = R C B ℓ B + t C B . 记
ℓ C = [ X C Y C Z C ] , Z C > 0. \ell_C=
\begin{bmatrix}
X_C\\Y_C\\Z_C
\end{bmatrix},
\qquad Z_C>0. ℓ C = X C Y C Z C , Z C > 0. 采用理想针孔模型，像素预测为
π ( ℓ C ) = [ f x X C / Z C + c x f y Y C / Z C + c y ] . \pi(\ell_C)=
\begin{bmatrix}
f_xX_C/Z_C+c_x\\
f_yY_C/Z_C+c_y
\end{bmatrix}. π ( ℓ C ) = [ f x X C / Z C + c x f y Y C / Z C + c y ] . 实际相机观测为
z u = π ( ℓ C ) + n u , z_u=\pi(\ell_C)+n_u, z u = π ( ℓ C ) + n u , 其中 z u = [ u , v ] ⊤ z_u=[u,v]^\top z u = [ u , v ] ⊤ n u n_u n u
ℓ ^ B = R ^ W B ⊤ ( P W − p ^ W ) , \hat\ell_B=\hat R_{WB}^{\top}(P_W-\hat p_W), ℓ ^ B = R ^ W B ⊤ ( P W − p ^ W ) , ℓ ^ C = R C B ℓ ^ B + t C B , \hat\ell_C=R_{CB}\hat\ell_B+t_{CB}, ℓ ^ C = R C B ℓ ^ B + t C B , z ^ u = π ( ℓ ^ C ) . \hat z_u=\pi(\hat\ell_C). z ^ u = π ( ℓ ^ C ) . 本文继续采用实际观测减预测观测的残差：
r u = z u − z ^ u . r_u=z_u-\hat z_u. r u = z u − z ^ u . 8.6.2 先求相机系点坐标对状态误差的导数
这一步先不做像素投影，只研究 ℓ C \ell_C ℓ C
ℓ B = R W B ⊤ ( P W − p W ) = Exp ( − [ δ θ ] × ) R ^ W B ⊤ ( P W − p ^ W − δ p ) . \begin{aligned}
\ell_B
&=R_{WB}^{\top}(P_W-p_W)\\
&=\operatorname{Exp}(-[\delta\theta]_\times)
\hat R_{WB}^{\top}(P_W-\hat p_W-\delta p).
\end{aligned} ℓ B = R W B ⊤ ( P W − p W ) = Exp ( − [ δ θ ] × ) R ^ W B ⊤ ( P W − p ^ W − δ p ) . 定义名义 IMU 系点坐标
ℓ ^ B ≜ R ^ W B ⊤ ( P W − p ^ W ) . \hat\ell_B\triangleq
\hat R_{WB}^{\top}(P_W-\hat p_W). ℓ ^ B ≜ R ^ W B ⊤ ( P W − p ^ W ) . 于是
ℓ B = Exp ( − [ δ θ ] × ) ( ℓ ^ B − R ^ W B ⊤ δ p ) . \ell_B
=\operatorname{Exp}(-[\delta\theta]_\times)
\left(\hat\ell_B-\hat R_{WB}^{\top}\delta p\right). ℓ B = Exp ( − [ δ θ ] × ) ( ℓ ^ B − R ^ W B ⊤ δ p ) . 使用
Exp ( − [ δ θ ] × ) ≈ I − [ δ θ ] × , \operatorname{Exp}(-[\delta\theta]_\times)
\approx I-[\delta\theta]_\times, Exp ( − [ δ θ ] × ) ≈ I − [ δ θ ] × , 得到
ℓ B ≈ ( I − [ δ θ ] × ) ( ℓ ^ B − R ^ W B ⊤ δ p ) ≈ ℓ ^ B − [ δ θ ] × ℓ ^ B − R ^ W B ⊤ δ p . \begin{aligned}
\ell_B
&\approx
\left(I-[\delta\theta]_\times\right)
\left(\hat\ell_B-\hat R_{WB}^{\top}\delta p\right)\\
&\approx
\hat\ell_B
-[\delta\theta]_\times\hat\ell_B
-\hat R_{WB}^{\top}\delta p.
\end{aligned} ℓ B ≈ ( I − [ δ θ ] × ) ( ℓ ^ B − R ^ W B ⊤ δ p ) ≈ ℓ ^ B − [ δ θ ] × ℓ ^ B − R ^ W B ⊤ δ p . 最后一项中的
[ δ θ ] × R ^ W B ⊤ δ p [\delta\theta]_\times\hat R_{WB}^{\top}\delta p [ δ θ ] × R ^ W B ⊤ δ p 包含两个误差量，是二阶项，需要舍去。再使用叉乘交换关系
− [ δ θ ] × ℓ ^ B = [ ℓ ^ B ] × δ θ , -[\delta\theta]_\times\hat\ell_B
=[\hat\ell_B]_\times\delta\theta, − [ δ θ ] × ℓ ^ B = [ ℓ ^ B ] × δ θ , 可得
ℓ B ≈ ℓ ^ B + [ ℓ ^ B ] × δ θ − R ^ W B ⊤ δ p . \boxed{
\ell_B
\approx
\hat\ell_B
+[\hat\ell_B]_\times\delta\theta
-\hat R_{WB}^{\top}\delta p.
} ℓ B ≈ ℓ ^ B + [ ℓ ^ B ] × δ θ − R ^ W B ⊤ δ p . 因此，IMU 系点坐标的一阶变化为
δ ℓ B = [ ℓ ^ B ] × δ θ − R ^ W B ⊤ δ p . \delta\ell_B
=[\hat\ell_B]_\times\delta\theta
-\hat R_{WB}^{\top}\delta p. δ ℓ B = [ ℓ ^ B ] × δ θ − R ^ W B ⊤ δ p . 接着通过固定外参变到相机系：
ℓ C = R C B ℓ B + t C B ≈ ℓ ^ C + R C B [ ℓ ^ B ] × δ θ − R C B R ^ W B ⊤ δ p . \begin{aligned}
\ell_C
&=R_{CB}\ell_B+t_{CB}\\
&\approx
\hat\ell_C
+R_{CB}[\hat\ell_B]_\times\delta\theta
-R_{CB}\hat R_{WB}^{\top}\delta p.
\end{aligned} ℓ C = R C B ℓ B + t C B ≈ ℓ ^ C + R C B [ ℓ ^ B ] × δ θ − R C B R ^ W B ⊤ δ p . 所以
δ ℓ C = R C B [ ℓ ^ B ] × δ θ − R C B R ^ W B ⊤ δ p . \boxed{
\delta\ell_C
=R_{CB}[\hat\ell_B]_\times\delta\theta
-R_{CB}\hat R_{WB}^{\top}\delta p.
} δ ℓ C = R C B [ ℓ ^ B ] × δ θ − R C B R ^ W B ⊤ δ p . 先得到这一步很重要。透视投影只是把三维相机点再映射到二维像素；如果坐标变换的 Jacobian 已经写错，后面的投影矩阵再正确也没有用。
8.6.3 再求像素对相机点的导数
对针孔投影
π ( ℓ C ) = [ f x X C / Z C + c x f y Y C / Z C + c y ] \pi(\ell_C)=
\begin{bmatrix}
f_xX_C/Z_C+c_x\\
f_yY_C/Z_C+c_y
\end{bmatrix} π ( ℓ C ) = [ f x X C / Z C + c x f y Y C / Z C + c y ] 求一阶导数。对第一行分别求 X C X_C X C Y C Y_C Y C Z C Z_C Z C
∂ u ∂ X C = f x Z C , ∂ u ∂ Y C = 0 , ∂ u ∂ Z C = − f x X C Z C 2 . \frac{\partial u}{\partial X_C}=\frac{f_x}{Z_C},
\qquad
\frac{\partial u}{\partial Y_C}=0,
\qquad
\frac{\partial u}{\partial Z_C}=-\frac{f_xX_C}{Z_C^2}. ∂ X C ∂ u = Z C f x , ∂ Y C ∂ u = 0 , ∂ Z C ∂ u = − Z C 2 f x X C . 对第二行：
∂ v ∂ X C = 0 , ∂ v ∂ Y C = f y Z C , ∂ v ∂ Z C = − f y Y C Z C 2 . \frac{\partial v}{\partial X_C}=0,
\qquad
\frac{\partial v}{\partial Y_C}=\frac{f_y}{Z_C},
\qquad
\frac{\partial v}{\partial Z_C}=-\frac{f_yY_C}{Z_C^2}. ∂ X C ∂ v = 0 , ∂ Y C ∂ v = Z C f y , ∂ Z C ∂ v = − Z C 2 f y Y C . 在名义相机点 ℓ ^ C = [ X ^ C , Y ^ C , Z ^ C ] ⊤ \hat\ell_C=[\hat X_C,\hat Y_C,\hat Z_C]^\top ℓ ^ C = [ X ^ C , Y ^ C , Z ^ C ] ⊤
J π ( ℓ ^ C ) = [ f x Z ^ C 0 − f x X ^ C Z ^ C 2 0 f y Z ^ C − f y Y ^ C Z ^ C 2 ] . \boxed{
J_\pi(\hat\ell_C)
=\begin{bmatrix}
\dfrac{f_x}{\hat Z_C}&0&-\dfrac{f_x\hat X_C}{\hat Z_C^2}\\[6pt]
0&\dfrac{f_y}{\hat Z_C}&-\dfrac{f_y\hat Y_C}{\hat Z_C^2}
\end{bmatrix}.
} J π ( ℓ ^ C ) = Z ^ C f x 0 0 Z ^ C f y − Z ^ C 2 f x X ^ C − Z ^ C 2 f y Y ^ C . 它是一个 2 × 3 2\times3 2 × 3
δ z u = J π ( ℓ ^ C ) δ ℓ C . \delta z_u
=J_\pi(\hat\ell_C)\delta\ell_C. δ z u = J π ( ℓ ^ C ) δ ℓ C . 这里出现的 1 / Z ^ C 1/\hat Z_C 1/ Z ^ C 1 / Z ^ C 2 1/\hat Z_C^2 1/ Z ^ C 2 Z ^ C \hat Z_C Z ^ C
8.6.4 用链式法则组装 H H H
将相机点对误差状态的导数和投影 Jacobian 相乘：
δ z u = J π ( ℓ ^ C ) ( R C B [ ℓ ^ B ] × δ θ − R C B R ^ W B ⊤ δ p ) = J π ( ℓ ^ C ) R C B [ ℓ ^ B ] × δ θ − J π ( ℓ ^ C ) R C B R ^ W B ⊤ δ p . \begin{aligned}
\delta z_u
&=J_\pi(\hat\ell_C)
\left(
R_{CB}[\hat\ell_B]_\times\delta\theta
-R_{CB}\hat R_{WB}^{\top}\delta p
\right)\\
&=J_\pi(\hat\ell_C)R_{CB}[\hat\ell_B]_\times\delta\theta\\
&\quad-J_\pi(\hat\ell_C)R_{CB}\hat R_{WB}^{\top}\delta p.
\end{aligned} δ z u = J π ( ℓ ^ C ) ( R C B [ ℓ ^ B ] × δ θ − R C B R ^ W B ⊤ δ p ) = J π ( ℓ ^ C ) R C B [ ℓ ^ B ] × δ θ − J π ( ℓ ^ C ) R C B R ^ W B ⊤ δ p . 因为当前假设下像素观测不直接依赖速度和两个 IMU bias，所以
H u = [ J π ( ℓ ^ C ) R C B [ ℓ ^ B ] × − J π ( ℓ ^ C ) R C B R ^ W B ⊤ 0 0 0 ] . \boxed{
H_u
=\begin{bmatrix}
J_\pi(\hat\ell_C)R_{CB}[\hat\ell_B]_\times
&-J_\pi(\hat\ell_C)R_{CB}\hat R_{WB}^{\top}
&0
&0
&0
\end{bmatrix}.
} H u = [ J π ( ℓ ^ C ) R C B [ ℓ ^ B ] × − J π ( ℓ ^ C ) R C B R ^ W B ⊤ 0 0 0 ] . 矩阵尺寸为
H u ∈ R 2 × 15 . H_u\in\mathbb R^{2\times15}. H u ∈ R 2 × 15 . 将它代回观测残差：
r u ≈ H u δ x + n u . \boxed{
r_u\approx H_u\delta x+n_u.
} r u ≈ H u δ x + n u . 这就是固定路标点的视觉重投影观测 Jacobian。它由两段组成：
J π R C B [ ℓ ^ B ] × ⏟ 姿态误差到像素误差 − J π R C B R ^ W B ⊤ ⏟ 位置误差到像素误差 . \underbrace{J_\pi R_{CB}[\hat\ell_B]_\times}_{\text{姿态误差到像素误差}}
\qquad
\underbrace{-J_\pi R_{CB}\hat R_{WB}^{\top}}_{\text{位置误差到像素误差}}. 姿态误差到像素误差 J π R C B [ ℓ ^ B ] × 位置误差到像素误差 − J π R C B R ^ W B ⊤ . 姿态误差先让点在 IMU 系中发生旋转位移，位置误差则先通过 R ^ W B ⊤ \hat R_{WB}^{\top} R ^ W B ⊤
8.6.5 为什么没有速度和 bias block
在这个观测时刻，单个图像像素由当前相机位姿和地图点决定。给定位姿不变，速度、陀螺仪 bias、加速度计 bias 不会直接改变当前像素预测，因此 H u H_u H u
这和位置观测的情况相同：H H H
K = P − H u ⊤ ( H u P − H u ⊤ + Σ u ) − 1 K=P^-H_u^\top
\left(H_uP^-H_u^\top+\Sigma_u\right)^{-1} K = P − H u ⊤ ( H u P − H u ⊤ + Σ u ) − 1 间接修正速度和 bias。
8.6.6 如果地图点也在状态中
上面的 P W P_W P W
P W = P ^ W + δ P W . P_W=\hat P_W+\delta P_W. P W = P ^ W + δ P W . 由
ℓ B = R W B ⊤ ( P W − p W ) \ell_B=R_{WB}^{\top}(P_W-p_W) ℓ B = R W B ⊤ ( P W − p W ) 可得新增的一阶项
δ ℓ C ⊃ R C B R ^ W B ⊤ δ P W . \delta\ell_C
\supset
R_{CB}\hat R_{WB}^{\top}\delta P_W. δ ℓ C ⊃ R C B R ^ W B ⊤ δ P W . 因此，若扩展状态包含地图点，视觉观测 Jacobian 还要增加
H P W = J π ( ℓ ^ C ) R C B R ^ W B ⊤ . H_{P_W}
=J_\pi(\hat\ell_C)R_{CB}\hat R_{WB}^{\top}. H P W = J π ( ℓ ^ C ) R C B R ^ W B ⊤ . 完整的视觉 SLAM 系统通常还会处理地图点参数化、逆深度、滑动窗口边缘化和相机外参。那些内容会改变状态维度和 Jacobian 的列，但不会改变这里的推导顺序。
8.6.7 相机外参和畸变模型
如果 R C B R_{CB} R C B t C B t_{CB} t C B δ ℓ C \delta\ell_C δ ℓ C H u H_u H u π \pi π
H u = ∂ π ∂ ℓ C ∂ ℓ C ∂ δ x . H_u
=\frac{\partial\pi}{\partial\ell_C}
\frac{\partial\ell_C}{\partial\delta x}. H u = ∂ ℓ C ∂ π ∂ δ x ∂ ℓ C . 工程实现中可以先用无畸变模型验证坐标和符号，再把畸变 Jacobian 接到投影部分。不要同时改变坐标变换、外参方向和畸变公式，否则很难判断误差来自哪一层。
8.6.8 重投影误差中最容易错的三个符号
第一，世界点到 IMU 系使用的是
R W B ⊤ ( P W − p W ) , R_{WB}^{\top}(P_W-p_W), R W B ⊤ ( P W − p W ) , 不是 R W B ( P W − p W ) R_{WB}(P_W-p_W) R W B ( P W − p W )
δ ℓ B = [ ℓ ^ B ] × δ θ − R ^ W B ⊤ δ p . \delta\ell_B
=[\hat\ell_B]_\times\delta\theta
-\hat R_{WB}^{\top}\delta p. δ ℓ B = [ ℓ ^ B ] × δ θ − R ^ W B ⊤ δ p . 第三，本文残差是实际像素减预测像素。如果代码使用预测减实际，整个 H u H_u H u 2 × 15 2\times15 2 × 15
9. 这些例子可以归纳成一个写 H H H
面对一个新观测，可以按下面的顺序操作。
第一步：写出物理观测模型
先不要考虑 Kalman Filter，直接写传感器理想情况下应该读到什么：
z = h ( X ) + n . z=h(\mathcal X)+n. z = h ( X ) + n . 例如：
GNSS 位置：h = p W h=p_W h = p W
世界系速度：h = v W h=v_W h = v W
机体系重力：h = R W B ⊤ g W h=R_{WB}^{\top}g_W h = R W B ⊤ g W
LiDAR 点到平面：h = n W ⊤ ( p W + R W B q B ) h=n_W^\top(p_W+R_{WB}q_B) h = n W ⊤ ( p W + R W B q B )
如果这一步的坐标系或符号没有说清楚，后面很难得到可靠的 Jacobian。
第二步：写出预测观测和残差
用预测名义状态计算
z ^ = h ( X ^ − ) , \hat z=h(\hat{\mathcal X}^-), z ^ = h ( X ^ − ) , 再固定残差方向
r = z − z ^ . r=z-\hat z. r = z − z ^ . 姿态等流形观测需要选择合适的 ⊟ \boxminus ⊟
第三步：把真实状态替换成名义状态加误差
统一代入
R = R ^ Exp ( [ δ θ ] × ) , p = p ^ + δ p , v = v ^ + δ v , b g = b ^ g + δ b g , b a = b ^ a + δ b a . \begin{aligned}
R&=\hat R\operatorname{Exp}([\delta\theta]_\times),\\
p&=\hat p+\delta p,\\
v&=\hat v+\delta v,\\
b_g&=\hat b_g+\delta b_g,\\
b_a&=\hat b_a+\delta b_a.
\end{aligned} R p v b g b a = R ^ Exp ([ δ θ ] × ) , = p ^ + δ p , = v ^ + δ v , = b ^ g + δ b g , = b ^ a + δ b a . 对旋转至少记住两条一阶公式：
Exp ( [ δ θ ] × ) ≈ I + [ δ θ ] × , \operatorname{Exp}([\delta\theta]_\times)
\approx I+[\delta\theta]_\times, Exp ([ δ θ ] × ) ≈ I + [ δ θ ] × , Exp ( − [ δ θ ] × ) x ≈ x + [ x ] × δ θ . \operatorname{Exp}(-[\delta\theta]_\times)x
\approx x+[x]_\times\delta\theta. Exp ( − [ δ θ ] × ) x ≈ x + [ x ] × δ θ . 第四步：只保留误差的一阶项
展开后，保留只含一个误差量的项，舍去
δ θ δ p , δ θ δ b a , δ θ 2 \delta\theta\,\delta p,
\quad
\delta\theta\,\delta b_a,
\quad
\delta\theta^2 δ θ δ p , δ θ δ b a , δ θ 2 等二阶项。
第五步：按状态排列组装 block
最后把残差写成
r ≈ H θ δ θ + H p δ p + H v δ v + H b g δ b g + H b a δ b a + n . r\approx
H_\theta\delta\theta
+H_p\delta p
+H_v\delta v
+H_{b_g}\delta b_g
+H_{b_a}\delta b_a+n. r ≈ H θ δ θ + H p δ p + H v δ v + H b g δ b g + H b a δ b a + n . 将五个 block 横向拼接，才得到完整的 H H H
9.1 视觉重投影误差完整走一遍流程
视觉重投影误差可以把上面的五步完整串起来。假设世界系地图点 P W P_W P W
9.1.1 写观测模型、预测观测和残差
世界点先从世界系变到 IMU 系，再变到相机系：
ℓ B = R W B ⊤ ( P W − p W ) , \ell_B=R_{WB}^{\top}(P_W-p_W), ℓ B = R W B ⊤ ( P W − p W ) , ℓ C = R C B ℓ B + t C B . \ell_C=R_{CB}\ell_B+t_{CB}. ℓ C = R C B ℓ B + t C B . 设
ℓ C = [ X C Y C Z C ] , \ell_C=
\begin{bmatrix}
X_C\\Y_C\\Z_C
\end{bmatrix}, ℓ C = X C Y C Z C , 理想针孔投影为
π ( ℓ C ) = [ f x X C / Z C + c x f y Y C / Z C + c y ] . \pi(\ell_C)=
\begin{bmatrix}
f_xX_C/Z_C+c_x\\
f_yY_C/Z_C+c_y
\end{bmatrix}. π ( ℓ C ) = [ f x X C / Z C + c x f y Y C / Z C + c y ] . 因此观测模型是
z u = π ( ℓ C ) + n u . z_u=\pi(\ell_C)+n_u. z u = π ( ℓ C ) + n u . 用预测名义状态计算
ℓ ^ B = R ^ W B ⊤ ( P W − p ^ W ) , \hat\ell_B=\hat R_{WB}^{\top}(P_W-\hat p_W), ℓ ^ B = R ^ W B ⊤ ( P W − p ^ W ) , ℓ ^ C = R C B ℓ ^ B + t C B , z ^ u = π ( ℓ ^ C ) , \hat\ell_C=R_{CB}\hat\ell_B+t_{CB},
\qquad
\hat z_u=\pi(\hat\ell_C), ℓ ^ C = R C B ℓ ^ B + t C B , z ^ u = π ( ℓ ^ C ) , 并采用实际像素减预测像素的残差：
r u = z u − z ^ u . r_u=z_u-\hat z_u. r u = z u − z ^ u . 9.1.2 代入误差状态并展开坐标变换
按照本文的右侧姿态误差和加性位置误差：
R W B = R ^ W B Exp ( [ δ θ ] × ) , p W = p ^ W + δ p . R_{WB}=\hat R_{WB}\operatorname{Exp}([\delta\theta]_\times),
\qquad
p_W=\hat p_W+\delta p. R W B = R ^ W B Exp ([ δ θ ] × ) , p W = p ^ W + δ p . 真实的 IMU 系点坐标为
ℓ B = R W B ⊤ ( P W − p W ) = Exp ( − [ δ θ ] × ) R ^ W B ⊤ ( P W − p ^ W − δ p ) = Exp ( − [ δ θ ] × ) ( ℓ ^ B − R ^ W B ⊤ δ p ) . \begin{aligned}
\ell_B
&=R_{WB}^{\top}(P_W-p_W)\\
&=\operatorname{Exp}(-[\delta\theta]_\times)
\hat R_{WB}^{\top}(P_W-\hat p_W-\delta p)\\
&=\operatorname{Exp}(-[\delta\theta]_\times)
\left(\hat\ell_B-\hat R_{WB}^{\top}\delta p\right).
\end{aligned} ℓ B = R W B ⊤ ( P W − p W ) = Exp ( − [ δ θ ] × ) R ^ W B ⊤ ( P W − p ^ W − δ p ) = Exp ( − [ δ θ ] × ) ( ℓ ^ B − R ^ W B ⊤ δ p ) . 使用
Exp ( − [ δ θ ] × ) ≈ I − [ δ θ ] × \operatorname{Exp}(-[\delta\theta]_\times)
\approx I-[\delta\theta]_\times Exp ( − [ δ θ ] × ) ≈ I − [ δ θ ] × 并舍去姿态误差与位置误差的乘积，得到
ℓ B ≈ ℓ ^ B − [ δ θ ] × ℓ ^ B − R ^ W B ⊤ δ p = ℓ ^ B + [ ℓ ^ B ] × δ θ − R ^ W B ⊤ δ p . \begin{aligned}
\ell_B
&\approx \hat\ell_B
-[\delta\theta]_\times\hat\ell_B
-\hat R_{WB}^{\top}\delta p\\
&=\hat\ell_B
+[\hat\ell_B]_\times\delta\theta
-\hat R_{WB}^{\top}\delta p.
\end{aligned} ℓ B ≈ ℓ ^ B − [ δ θ ] × ℓ ^ B − R ^ W B ⊤ δ p = ℓ ^ B + [ ℓ ^ B ] × δ θ − R ^ W B ⊤ δ p . 经过固定外参变换：
δ ℓ C = ℓ C − ℓ ^ C = R C B [ ℓ ^ B ] × δ θ − R C B R ^ W B ⊤ δ p . \begin{aligned}
\delta\ell_C
&=\ell_C-\hat\ell_C\\
&=R_{CB}[\hat\ell_B]_\times\delta\theta
-R_{CB}\hat R_{WB}^{\top}\delta p.
\end{aligned} δ ℓ C = ℓ C − ℓ ^ C = R C B [ ℓ ^ B ] × δ θ − R C B R ^ W B ⊤ δ p . 这一步给出三维相机点对误差状态的 Jacobian：
∂ ℓ C ∂ δ x = [ R C B [ ℓ ^ B ] × − R C B R ^ W B ⊤ 0 0 0 ] . \frac{\partial\ell_C}{\partial\delta x}
=\begin{bmatrix}
R_{CB}[\hat\ell_B]_\times
&-R_{CB}\hat R_{WB}^{\top}
&0&0&0
\end{bmatrix}. ∂ δ x ∂ ℓ C = [ R C B [ ℓ ^ B ] × − R C B R ^ W B ⊤ 0 0 0 ] . 9.1.3 展开透视投影并用链式法则组装
针孔投影对相机点的 Jacobian 为
J π ( ℓ ^ C ) = [ f x Z ^ C 0 − f x X ^ C Z ^ C 2 0 f y Z ^ C − f y Y ^ C Z ^ C 2 ] . J_\pi(\hat\ell_C)
=\begin{bmatrix}
\dfrac{f_x}{\hat Z_C}&0&-\dfrac{f_x\hat X_C}{\hat Z_C^2}\\[6pt]
0&\dfrac{f_y}{\hat Z_C}&-\dfrac{f_y\hat Y_C}{\hat Z_C^2}
\end{bmatrix}. J π ( ℓ ^ C ) = Z ^ C f x 0 0 Z ^ C f y − Z ^ C 2 f x X ^ C − Z ^ C 2 f y Y ^ C . 于是像素误差的一阶项为
δ z u = J π ( ℓ ^ C ) δ ℓ C = J π ( ℓ ^ C ) R C B [ ℓ ^ B ] × δ θ − J π ( ℓ ^ C ) R C B R ^ W B ⊤ δ p . \begin{aligned}
\delta z_u
&=J_\pi(\hat\ell_C)\delta\ell_C\\
&=J_\pi(\hat\ell_C)R_{CB}[\hat\ell_B]_\times\delta\theta
-J_\pi(\hat\ell_C)R_{CB}\hat R_{WB}^{\top}\delta p.
\end{aligned} δ z u = J π ( ℓ ^ C ) δ ℓ C = J π ( ℓ ^ C ) R C B [ ℓ ^ B ] × δ θ − J π ( ℓ ^ C ) R C B R ^ W B ⊤ δ p . 因此，按
δ x = [ δ θ δ p δ v δ b g δ b a ] \delta x=
\begin{bmatrix}
\delta\theta\\\delta p\\\delta v\\\delta b_g\\\delta b_a
\end{bmatrix} δ x = δ θ δ p δ v δ b g δ b a 排列，视觉重投影观测的 Jacobian 为
H u = [ J π ( ℓ ^ C ) R C B [ ℓ ^ B ] × − J π ( ℓ ^ C ) R C B R ^ W B ⊤ 0 0 0 ] . \boxed{
H_u=\begin{bmatrix}
J_\pi(\hat\ell_C)R_{CB}[\hat\ell_B]_\times
&-J_\pi(\hat\ell_C)R_{CB}\hat R_{WB}^{\top}
&0&0&0
\end{bmatrix}.
} H u = [ J π ( ℓ ^ C ) R C B [ ℓ ^ B ] × − J π ( ℓ ^ C ) R C B R ^ W B ⊤ 0 0 0 ] . 因此，视觉残差的线性化形式为
r u ≈ H u δ x + n u . r_u\approx H_u\delta x+n_u. r u ≈ H u δ x + n u . 它是一个 2 × 15 2\times15 2 × 15 J π J_\pi J π
如果地图点也在状态中，只需继续对
ℓ C = R C B R W B ⊤ ( P W − p W ) + t C B \ell_C=R_{CB}R_{WB}^{\top}(P_W-p_W)+t_{CB} ℓ C = R C B R W B ⊤ ( P W − p W ) + t C B 对 δ P W \delta P_W δ P W H u H_u H u
H P W = J π ( ℓ ^ C ) R C B R ^ W B ⊤ . H_{P_W}=J_\pi(\hat\ell_C)R_{CB}\hat R_{WB}^{\top}. H P W = J π ( ℓ ^ C ) R C B R ^ W B ⊤ . 相机外参或畸变参数进入状态时，也沿着同一条链式法则继续增加对应 block。由此可见，复杂观测并没有改变写 H H H
10. 观测更新之后：误差注入和 reset
10.1 先更新局部误差
使用残差和 H H H
δ x ^ = K r . \delta\hat x=Kr. δ x ^ = K r . 把它展开为
δ x ^ = [ δ θ ^ δ p ^ δ v ^ δ b ^ g δ b ^ a ] . \delta\hat x=
\begin{bmatrix}
\delta\hat\theta\\
\delta\hat p\\
\delta\hat v\\
\delta\hat b_g\\
\delta\hat b_a
\end{bmatrix}. δ x ^ = δ θ ^ δ p ^ δ v ^ δ b ^ g δ b ^ a . 这些量仍然属于预测名义状态附近的局部坐标。
10.2 注入名义状态
对于右侧姿态误差，姿态注入为
R ^ + = R ^ − Exp ( [ δ θ ^ ] × ) . \hat R^+
=\hat R^-
\operatorname{Exp}([\delta\hat\theta]_\times). R ^ + = R ^ − Exp ([ δ θ ^ ] × ) . 其余状态直接相加：
p ^ + = p ^ − + δ p ^ , v ^ + = v ^ − + δ v ^ , b ^ g + = b ^ g − + δ b ^ g , b ^ a + = b ^ a − + δ b ^ a . \begin{aligned}
\hat p^+&=\hat p^-+\delta\hat p,\\
\hat v^+&=\hat v^-+\delta\hat v,\\
\hat b_g^+&=\hat b_g^-+\delta\hat b_g,\\
\hat b_a^+&=\hat b_a^-+\delta\hat b_a.
\end{aligned} p ^ + v ^ + b ^ g + b ^ a + = p ^ − + δ p ^ , = v ^ − + δ v ^ , = b ^ g − + δ b ^ g , = b ^ a − + δ b ^ a . 不能把 H H H H H H δ x ^ \delta\hat x δ x ^
10.3 为什么还需要 reset
误差注入后，名义状态已经移动。误差状态的零点也随之改变。对加性状态，一阶近似下有
δ x n e w ≈ δ x o l d − δ x ^ . \delta x_{\mathrm{new}}
\approx
\delta x_{\mathrm{old}}-\delta\hat x. δ x new ≈ δ x old − δ x ^ . 姿态则需要使用旋转复合：
Exp ( [ δ θ n e w ] × ) = Exp ( − [ δ θ ^ ] × ) Exp ( [ δ θ o l d ] × ) . \operatorname{Exp}([\delta\theta_{\mathrm{new}}]_\times)
=
\operatorname{Exp}(-[\delta\hat\theta]_\times)
\operatorname{Exp}([\delta\theta_{\mathrm{old}}]_\times). Exp ([ δ θ new ] × ) = Exp ( − [ δ θ ^ ] × ) Exp ([ δ θ old ] × ) . 因此
δ θ n e w = Log ( Exp ( − [ δ θ ^ ] × ) Exp ( [ δ θ o l d ] × ) ) ∨ . \delta\theta_{\mathrm{new}}
=\operatorname{Log}\left(
\operatorname{Exp}(-[\delta\hat\theta]_\times)
\operatorname{Exp}([\delta\theta_{\mathrm{old}}]_\times)
\right)^\vee. δ θ new = Log ( Exp ( − [ δ θ ^ ] × ) Exp ([ δ θ old ] × ) ) ∨ . 对小角度，常见的一阶近似是
δ θ n e w ≈ δ θ o l d − δ θ ^ . \delta\theta_{\mathrm{new}}
\approx
\delta\theta_{\mathrm{old}}-\delta\hat\theta. δ θ new ≈ δ θ old − δ θ ^ . 10.4 协方差也要换坐标
Kalman 更新得到的协方差通常对应注入前的误差坐标，记为 P ~ + \widetilde P^+ P +
P + = J r e s e t P ~ + J r e s e t ⊤ . \boxed{
P^+=J_{\mathrm{reset}}\widetilde P^+J_{\mathrm{reset}}^\top.
} P + = J reset P + J reset ⊤ . 对普通加性状态，相关 Jacobian 常近似为单位矩阵；旋转部分则取决于误差定义、注入方式和采用的近似阶数。不能因为当前的 δ θ ^ \delta\hat\theta δ θ ^
这一步和 H H H H H H P P P H H H
11. 用三个数值检查发现大多数符号错误
推导完成后，不妨用有限差分检查 H H H
11.1 有限差分检查
对误差状态的第 j j j ϵ \epsilon ϵ e j e_j e j
h + = h ( X ^ ⊞ ϵ e j ) , h − = h ( X ^ ⊞ ( − ϵ e j ) ) . h_+=h(\hat{\mathcal X}\boxplus\epsilon e_j),
\qquad
h_-=h(\hat{\mathcal X}\boxplus(-\epsilon e_j)). h + = h ( X ^ ⊞ ϵ e j ) , h − = h ( X ^ ⊞ ( − ϵ e j )) . 则数值 Jacobian 的第 j j j
H : , j n u m ≈ h + − h − 2 ϵ . H_{:,j}^{\mathrm{num}}
\approx
\frac{h_+-h_-}{2\epsilon}. H : , j num ≈ 2 ϵ h + − h − . 姿态观测不能直接对旋转矩阵做减法。需要先把两个预测观测转换为同一个观测残差坐标，再做中心差分。对于普通向量观测，上式可以直接使用。
比较解析 Jacobian 和数值 Jacobian：
∥ H a n a l y t i c − H n u m ∥ \lVert H^{\mathrm{analytic}}-H^{\mathrm{num}}\rVert ∥ H analytic − H num ∥ 应该在合理的数值误差范围内较小。ϵ \epsilon ϵ ϵ \epsilon ϵ
11.2 重力观测的 yaw 检查
对于纯重力向量观测，应该满足
[ g ^ B ] × g ^ B = 0. [\hat g_B]_\times\hat g_B=0. [ g ^ B ] × g ^ B = 0. 如果代码声称重力观测能够约束绕重力方向的旋转，通常说明观测模型或可观性判断出了问题。
11.3 点到平面的切向检查
对平面内切向量 t W t_W t W
n W ⊤ t W = 0. n_W^\top t_W=0. n W ⊤ t W = 0. 因此点到平面观测对沿平面切向平移的一阶 Jacobian 为零。这个检查可以快速发现法向量是否被错误地转到了别的坐标系，或者点到平面的残差符号是否混乱。
11.4 维度和单位检查
每次构造观测 Jacobian 时至少检查：
H H H m × 15 m\times15 m × 15 残差维度是否为 m m m
Σ z \Sigma_z Σ z m × m m\times m m × m S = H P − H ⊤ + Σ z S=HP^-H^\top+\Sigma_z S = H P − H ⊤ + Σ z m × m m\times m m × m 残差的单位是否和观测噪声协方差一致。
例如，点到平面残差是米，姿态残差是弧度，不能把它们混在一个残差向量中却不给出相应的噪声尺度。
12. 最容易混淆的几件事
12.1 把 H H H
F F F H H H F F F 15 × 15 15\times15 15 × 15 H H H
12.2 只看观测名称，不看输出坐标系
“速度观测”可能是世界系速度，也可能是机体系前向速度；“位置观测”可能是 IMU 原点，也可能是相机或 LiDAR 原点。传感器名称不能替代观测模型。
12.3 对旋转做普通减法
姿态残差应先构造相对旋转，再通过 Log \operatorname{Log} Log
12.4 把静止加速度计读数当成重力加速度
加速度计测量的是比力。按照本文模型，静止时理想读数为
− R W B ⊤ g W + b a , -R_{WB}^{\top}g_W+b_a, − R W B ⊤ g W + b a , 而不是 R W B ⊤ g W R_{WB}^{\top}g_W R W B ⊤ g W
12.5 忘记 bias 是否在观测模型中出现
纯位置观测的 H H H H H H H b a = I H_{b_a}=I H b a = I
12.6 用错误的残差方向，却只修改一处
如果残差从 z − z ^ z-\hat z z − z ^ z ^ − z \hat z-z z ^ − z H H H
12.7 忘记 reset 协方差
误差注入改变了姿态误差的切空间。只更新名义状态、不转换 P P P
13. 把一轮观测更新串起来
现在可以把 ESKF 的观测阶段完整写出来。
第一步：IMU 传播到观测时刻
上一篇得到的 F F F G G G
P − ≈ Φ P + Φ ⊤ + Q d . P^-\approx\Phi P^+\Phi^\top+Q_d. P − ≈ Φ P + Φ ⊤ + Q d . 同时，名义状态按照非线性 IMU 方程传播到 X ^ − \hat{\mathcal X}^- X ^ −
第二步：根据当前传感器写 h h h
例如位置观测：
h p ( X ^ − ) = p ^ − . h_p(\hat{\mathcal X}^-)=\hat p^-. h p ( X ^ − ) = p ^ − . 点到平面观测：
h π ( X ^ − ) = n W ⊤ ( p ^ − + R ^ − q B ) . h_\pi(\hat{\mathcal X}^-)
=n_W^\top(\hat p^-+\hat R^-q_B). h π ( X ^ − ) = n W ⊤ ( p ^ − + R ^ − q B ) . 第三步：计算残差和 H H H
r = z − h ( X ^ − ) , r=z-h(\hat{\mathcal X}^-), r = z − h ( X ^ − ) , H = ∂ h ( X ^ − ⊞ δ x ) ∂ δ x ∣ 0 . H=\left.
\frac{\partial h(\hat{\mathcal X}^-\boxplus\delta x)}
{\partial\delta x}
\right|_0. H = ∂ δ x ∂ h ( X ^ − ⊞ δ x ) 0 . H H H
第四步：更新误差状态
S = H P − H ⊤ + Σ z , S=HP^-H^\top+\Sigma_z, S = H P − H ⊤ + Σ z , K = P − H ⊤ S − 1 , K=P^-H^\top S^{-1}, K = P − H ⊤ S − 1 , δ x ^ = K r . \delta\hat x=Kr. δ x ^ = K r . 第五步：注入并 reset
X ^ + = X ^ − ⊞ δ x ^ , \hat{\mathcal X}^+=\hat{\mathcal X}^-\boxplus\delta\hat x, X ^ + = X ^ − ⊞ δ x ^ , 再把注入前的后验协方差转换到新的误差坐标：
P + = J r e s e t P ~ + J r e s e t ⊤ . P^+=J_{\mathrm{reset}}\widetilde P^+J_{\mathrm{reset}}^\top. P + = J reset P + J reset ⊤ . 这样，下一次 IMU 到来时，新的名义状态和新的协方差正好位于同一个局部坐标约定下。
14. 从一条新观测到一块正确的 H H H
本文最值得留下的不是某个单独的矩阵，而是下面这条推导路线：
明确状态和所有坐标系的方向；
写出理想观测模型 z = h ( X ) + n z=h(\mathcal X)+n z = h ( X ) + n
用预测名义状态计算 z ^ = h ( X ^ − ) \hat z=h(\hat{\mathcal X}^-) z ^ = h ( X ^ − )
选定残差方向，例如 r = z − z ^ r=z-\hat z r = z − z ^
把 R = R ^ Exp ( [ δ θ ] × ) R=\hat R\operatorname{Exp}([\delta\theta]_\times) R = R ^ Exp ([ δ θ ] × ) h h h
舍去二阶小量，保留误差的一阶项；
按 [ δ θ , δ p , δ v , δ b g , δ b a ] [\delta\theta,\delta p,\delta v,\delta b_g,\delta b_a] [ δ θ , δ p , δ v , δ b g , δ b a ] H H H
用有限差分、秩和几何退化检查结果；
更新局部误差，注入名义状态，再 reset 协方差。
位置观测给出
H p = [ 0 I 0 0 0 ] , H_p=\begin{bmatrix}0&I&0&0&0\end{bmatrix}, H p = [ 0 I 0 0 0 ] , 世界系速度观测给出
H v = [ 0 0 I 0 0 ] , H_v=\begin{bmatrix}0&0&I&0&0\end{bmatrix}, H v = [ 0 0 I 0 0 ] , 纯重力向量观测给出
H g = [ [ g ^ B ] × 0 0 0 0 ] , H_g=\begin{bmatrix}[\hat g_B]_\times&0&0&0&0\end{bmatrix}, H g = [ [ g ^ B ] × 0 0 0 0 ] , 静止加速度计比力观测给出
H a = [ − [ g ^ B ] × 0 0 0 I ] , H_a=\begin{bmatrix}-[\hat g_B]_\times&0&0&0&I\end{bmatrix}, H a = [ − [ g ^ B ] × 0 0 0 I ] , 点到平面观测给出
H π = [ − n W ⊤ R ^ W B [ q B ] × n W ⊤ 0 0 0 ] . H_\pi=
\begin{bmatrix}
-n_W^\top\hat R_{WB}[q_B]_\times
&n_W^\top
&0&0&0
\end{bmatrix}. H π = [ − n W ⊤ R ^ W B [ q B ] × n W ⊤ 0 0 0 ] . 这些矩阵的形式看起来不同，但来源完全相同：先让误差状态改变预测观测，再读取预测观测的一阶变化。
下一步如果继续写 ESKF 系列，可以在这套观测 Jacobian 的基础上推导具体的视觉重投影误差，或者把 LiDAR 的点到平面观测扩展到 IESKF 的迭代更新。无论观测来自相机还是 LiDAR，先把观测模型和误差坐标写清楚，后面的矩阵就不会只剩下记忆。
参考文章
ESKF 误差动力学推导：从 IMU 模型到 F F F G G G Joan Solà, Quaternion kinematics for the error-state Kalman filter , arXiv:1711.02508, 2017. https://arxiv.org/abs/1711.02508
Joan Solà, Jérémie Deray, and Dinesh Atchuthan, A micro Lie theory for state estimation in robotics , arXiv:1812.01537, 2018. https://arxiv.org/abs/1812.01537
Wei Xu, Yixi Cai, Dongjiao He, Jiarong Lin, and Fu Zhang, FAST-LIO2: Fast Direct LiDAR-inertial Odometry , IEEE Transactions on Robotics, 38(4):2053–2070, 2022. https://arxiv.org/abs/2107.06829