四元数 (Quaternion)动力学左乘/右乘约定下之误差态 EKF 的连续线性化与离散化传播示例（11）

相关内容:四元数 (Quaternion)微分-从惯性系角速度推导四元数导数的矩阵表示（9）

约定

• 四元数采用 scalar-first : q = [ w , x , y , z ] T q=[w,x,y,z]^T q=[w,x,y,z]T.

• 真实陀螺仪测量模型： ω m = ω + b + n g \omega_m = \omega + b + n_g ωm=ω+b+ng，其中 b b b 是陀螺偏置， n g n_g ng 是测量噪声（零均值白噪声，连续谱协方差 Q g Q_{g} Qg）。

• 偏置模型： b ˙ = n b g \dot b = n_{bg} b˙=nbg （随机游走，噪声谱协方差 Q b g Q_{bg} Qbg）。

• 状态/误差态选择（常用误差态EKF）：

  • 估计量（nominal） q ^ , b ^ \hat q, \hat b q^,b^。
  • 误差态 δ x = [ δ ϕ ;    δ b ] ∈ R 6 \delta x = [\delta\boldsymbol{\phi};\; \delta b]\in\mathbb R^6 δx=[δϕ;δb]∈R6，其中 δ ϕ ∈ R 3 \delta\boldsymbol{\phi}\in\mathbb R^3 δϕ∈R3 是小角误差（与四元数的微小扰动关系 δ q ≈ [ 1 , 1 2 δ ϕ ] T \delta q \approx [1, \tfrac{1}{2}\delta\boldsymbol{\phi}]^T δq≈[1,21δϕ]T）， δ b = b − b ^ \delta b = b - \hat b δb=b−b^。

---

I. 右乘 / body-frame（ q ˙ = 1 2   q ⊗ ( 0 , ω ) \dot q = \tfrac12\, q\otimes(0,\omega) q˙=21q⊗(0,ω)）

1. 连续系统（真实 / 估计）

真实：

q ˙ = 1 2   q ⊗ ( 0 , ω ) , ω = ω m − b − n g . \dot q = \tfrac12\, q\otimes (0,\omega), \qquad \omega = \omega_m - b - n_g. q˙=21q⊗(0,ω),ω=ωm−b−ng.

估计（用估计偏置 b ^ \hat b b^、用测量 ω m \omega_m ωm）：

q ^ ˙ = 1 2   q ^ ⊗ ( 0 , ω m − b ^ ) . \dot{\hat q} = \tfrac12\, \hat q\otimes (0,\omega_m - \hat b). q^˙=21q^⊗(0,ωm−b^).

2. 误差态定义（右乘误差）

定义 右乘误差：

q = q ^ ⊗ δ q , δ q ≈ [ 1 1 2 δ ϕ ] , ∥ δ ϕ ∥ ≪ 1. q = \hat q \otimes \delta q, \qquad \delta q \approx \begin{bmatrix}1\\ \tfrac12 \delta\phi\end{bmatrix},\quad \|\delta\phi\|\ll1. q=q^⊗δq,δq≈[121δϕ],∥δϕ∥≪1.

偏置误差： δ b = b − b ^ \delta b = b - \hat b δb=b−b^.

（右乘误差是 EKF + 状态在机体系角速度常见约定，能得到标准线性化形式。）

3. 线性化（逐项理由在前面的对话中已有）------连续误差动力学

推导（省略重复四元数代数展开步骤，给出结果）得到标准线性误差动力学：

δ ϕ ˙ = − [ ω m − b ^ ] ×   δ ϕ    −    δ b    −    n g , δ b ˙ = n b g . \boxed{% \begin{aligned} \delta\dot\phi &= - [\omega_m - \hat b]\times \, \delta\phi \;-\; \delta b \;-\; n_g,\\ \delta\dot b &= n{bg}. \end{aligned} } δϕ˙δb˙=−[ωm−b^]×δϕ−δb−ng,=nbg.

解释：

• 矩阵 [ ⋅ ] × [\cdot]\times [⋅]× 是 3×3 叉乘（反对称）矩阵，满足 [ α ] × v = α × v [\alpha]\times v = \alpha\times v [α]×v=α×v。
• 负号来自右乘与四元数乘法顺序（参见之前证明）。
• 角速度项用了 ω m − b ^ \omega_m - \hat b ωm−b^（nominal used in linearization）。

把上面写成标准线性化形式

δ x ˙ = A   δ x + B   η , δ x = [ δ ϕ δ b ] , η = [ n g n b g ] , \delta\dot x = A \, \delta x + B \, \eta, \quad \delta x = \begin{bmatrix}\delta\phi\\ \delta b\end{bmatrix},\quad \eta = \begin{bmatrix} n_g \\ n_{bg}\end{bmatrix}, δx˙=Aδx+Bη,δx=[δϕδb],η=[ngnbg],

其中（维度： A A A 是 6 × 6 6\times6 6×6， B B B 是 6 × 6 6\times6 6×6）：

A body = ( [ ω m − b ^ ] ×    − I 3 0 3 × 3    0 3 × 3 ) , B body = ( I 3 0 3 × 3 0 3 × 3 I 3 ) . \boxed{% A_{\text{body}} = \begin{pmatrix} [\omega_m - \hat b]\times &\; -I{3} \\ 0_{3\times3} &\; 0_{3\times3} \end{pmatrix}, \qquad B_{\text{body}} = \begin{pmatrix} I_{3} & 0_{3\times3} \\ 0_{3\times3} & I_{3} \end{pmatrix}. } Abody=([ωm−b^]×03×3−I303×3),Bbody=(I303×303×3I3).

说明：

• B B B 反映进程噪声 η = [ n g , n b g ] \eta = [n_g, n_{bg}] η=[ng,nbg] 如何进入误差态。注意： n g n_g ng 在 δ ϕ ˙ \delta\dot\phi δϕ˙ 中以负号出现（参见测量模型），因此上面 top-left 是 − I 3 -I_3 −I3。

---

4. 连续→离散：状态转移矩阵 Φ \Phi Φ

要得到离散步长 Δ t \Delta t Δt 下的转移矩阵

δ x k + 1 = Φ ( Δ t )   δ x k + w k , \delta x_{k+1} = \Phi(\Delta t)\,\delta x_k + w_k, δxk+1=Φ(Δt)δxk+wk,

精确表达为矩阵指数：

Φ = exp ⁡ ( A Δ t ) . \Phi = \exp(A\Delta t). Φ=exp(AΔt).

由于 A A A 为块上三角（下左块为 0），能得到封闭块解（常见事实：若 A = ( A 11 A 12 0 0 ) A=\begin{pmatrix}A_{11}&A_{12}\\0&0\end{pmatrix} A=(A110A120),\ exp ⁡ ( A Δ t ) = ( e A 11 Δ t ∫ 0 Δ t e A 11 ( Δ t − s ) A 12   d s 0 I ) \exp(A\Delta t)=\begin{pmatrix}e^{A_{11}\Delta t}& \int_0^{\Delta t} e^{A_{11}(\Delta t-s)} A_{12}\, ds \\ 0 & I\end{pmatrix} exp(AΔt)=(eA11Δt0∫0ΔteA11(Δt−s)A12dsI)）。套用得到：

Φ body ( Δ t ) = ( Φ ϕ ϕ Φ ϕ b 0 3 × 3 I 3 ) , \boxed{% \Phi_{\text{body}}(\Delta t) = \begin{pmatrix} \Phi_{\phi\phi} & \Phi_{\phi b} \\ 0_{3\times3} & I_{3} \end{pmatrix}, } Φbody(Δt)=(Φϕϕ03×3ΦϕbI3),

其中

• Φ ϕ ϕ = exp ⁡ ( − [ ω m − b ^ ] × Δ t ) \displaystyle \Phi_{\phi\phi} = \exp\big(-[\omega_m - \hat b]_\times \Delta t\big) Φϕϕ=exp(−[ωm−b^]×Δt) （即绕角 ( ω m − b ^ ) Δ t (\omega_m-\hat b)\Delta t (ωm−b^)Δt 的旋转矩阵），
• Φ ϕ b = ∫ 0 Δ t exp ⁡ ( − [ ω m − b ^ ] × ( Δ t − s ) ) ( − I 3 )   d s . \displaystyle \Phi_{\phi b} = \int_0^{\Delta t} \exp\big(-[\omega_m - \hat b]_\times (\Delta t - s)\big) (-I_3) \, ds. Φϕb=∫0Δtexp(−[ωm−b^]×(Δt−s))(−I3)ds.

将积分用叉乘矩阵的闭式替代：当 Ω : = [ ω m − b ^ ] × \Omega := [\omega_m-\hat b]_\times Ω:=[ωm−b^]× 且若 ω : = ω m − b ^ \omega := \omega_m - \hat b ω:=ωm−b^ 的模非零，则

Φ ϕ b = −   Ω − 1   ( I 3 − e Ω Δ t ) \boxed{% \Phi_{\phi b} = -\,\Omega^{-1}\,\big(I_3 - e^{\Omega\Delta t}\big) } Φϕb=−Ω−1(I3−eΩΔt)

（因为 ∫ 0 Δ t e Ω s d s = Ω − 1 ( e Ω Δ t − I ) \int_0^{\Delta t} e^{\Omega s} ds = \Omega^{-1}(e^{\Omega\Delta t}-I) ∫0ΔteΩsds=Ω−1(eΩΔt−I), 改写成上面形式并调整符号）。换一种更常见的、基于右雅可比的表示：

Φ ϕ b = −   J r ( ω Δ t )   Δ t \Phi_{\phi b} = -\,J_r(\omega\Delta t)\,\Delta t Φϕb=−Jr(ωΔt)Δt

其中 J r ( ⋅ ) J_r(\cdot) Jr(⋅) 是 SO(3) 的右雅可比（与 Rodrigues / matrix exponential 相关）。在实现中，常用更直接的公式：

• 若 θ = ∥ ω ∥ Δ t \theta = \|\omega\| \Delta t θ=∥ω∥Δt 非零，令 u ^ = ω / ∥ ω ∥ \hat u = \omega/\|\omega\| u^=ω/∥ω∥, 则

  e − [ ω ] × Δ t = I − sin ⁡ θ   [ u ^ ] × + ( 1 − cos ⁡ θ )   [ u ^ ] × 2 . e^{-[\omega]\times \Delta t} = I - \sin\theta\,[\hat u]\times + (1-\cos\theta)\,[\hat u]_\times^2. e−[ω]×Δt=I−sinθ[u^]×+(1−cosθ)[u^]×2.

  并

  ∫ 0 Δ t e − [ ω ] × s d s = 1 ∥ ω ∥ ( I − e − [ ω ] × Δ t ) [ u ^ ] × − 1 \int_0^{\Delta t} e^{-[\omega]\times s} ds = \frac{1}{\|\omega\|}\Big(I - e^{-[\omega]\times \Delta t}\Big)[\hat u]_\times^{-1} ∫0Δte−[ω]×sds=∥ω∥1(I−e−[ω]×Δt)[u^]×−1

  or use the standard identity Ω − 1 ( I − e Ω Δ t ) \Omega^{-1}(I - e^{\Omega\Delta t}) Ω−1(I−eΩΔt).

实用实现提示：推荐直接计算 Φ ϕ ϕ = exp ⁡ ( − [ ω ] Δ t ) \Phi_{\phi\phi}= \exp(-[\omega]\Delta t) Φϕϕ=exp(−[ω]Δt) via Rodrigues (closed form) 或 Eigen::MatrixExponential；然后计算 Φ ϕ b = − Ω − 1 ( I − Φ ϕ ϕ ) \Phi_{\phi b} = -\Omega^{-1}(I - \Phi_{\phi\phi}) Φϕb=−Ω−1(I−Φϕϕ)。若 ∥ ω ∥ \|\omega\| ∥ω∥ 接近 0 要用级数展开避免数值不稳定（Taylor 展开）。

小角近似（常用、简单） ：

当角速率或 Δ t \Delta t Δt 很小，常用一阶近似：

Φ ϕ ϕ ≈ I 3 − [ ω ] Δ t , Φ ϕ b ≈ − I 3 Δ t + 1 2 [ ω ] Δ t 2 . \Phi_{\phi\phi} \approx I_3 - [\omega]\Delta t, \qquad \Phi_{\phi b} \approx -I_3 \Delta t + \tfrac12 [\omega] \Delta t^2. Φϕϕ≈I3−[ω]Δt,Φϕb≈−I3Δt+21[ω]Δt2.

并且右下块恒为 I 3 I_3 I3。

---

5. 离散过程噪声协方差 Q d Q_d Qd

连续噪声谱矩阵（输入噪声）为

Q c = d i a g ( Q g ,    Q b g ) ( 6 × 6 ) . Q_c = \mathrm{diag}(Q_g,\; Q_{bg}) \quad (6\times6). Qc=diag(Qg,Qbg)(6×6).

离散化后噪声协方差为

Q d = ∫ 0 Δ t Φ ( Δ t − s )    B    Q c    B T    Φ ( Δ t − s ) T   d s . \boxed{% Q_d = \int_0^{\Delta t} \Phi(\Delta t - s)\; B\; Q_c\; B^T\; \Phi(\Delta t - s)^T \, ds. } Qd=∫0ΔtΦ(Δt−s)BQcBTΦ(Δt−s)Tds.

由于 Φ \Phi Φ 是上三角形式，上式有解析块解（繁琐但可写出）。常见做法：

• 直接数值积分：在实现中常用数值积分或对角近似（当 Qc 不大时）。

• 闭式表达 （块形式）：若写成 2×2 块并利用 Φ \Phi Φ 解析形式，可以得到：

  • Q b b = Q b g Δ t Q_{bb} = Q_{bg}\Delta t Qbb=QbgΔt,
  • Q ϕ b = ∫ 0 Δ t Φ ϕ ϕ ( s )   ( − I )   Q b g   d s Q_{\phi b} = \int_0^{\Delta t} \Phi_{\phi\phi}(s)\,(-I)\,Q_{bg}\, ds Qϕb=∫0ΔtΦϕϕ(s)(−I)Qbgds etc.（具体展开较长，可按需给出）。

• 常用近似（工程上常用）：

  Q d ≈ ( Q g Δ t + 1 3 Q b g Δ t 3 1 2 Q b g Δ t 2 1 2 Q b g Δ t 2 Q b g Δ t ) Q_d \approx \begin{pmatrix} Q_g \Delta t + \tfrac{1}{3} Q_{bg} \Delta t^3 & \quad \tfrac12 Q_{bg} \Delta t^2 \\ \tfrac12 Q_{bg} \Delta t^2 & \quad Q_{bg}\Delta t \end{pmatrix} Qd≈(QgΔt+31QbgΔt321QbgΔt221QbgΔt2QbgΔt)

  （该近似给出了合理的尺度估计，实际需根据 B 与 Φ \Phi Φ 的形式略作调整。）

---

II. 左乘 / inertial-frame（ q ˙ = 1 2   ( 0 , ω ) ⊗ q \dot q = \tfrac12\,(0,\omega)\otimes q q˙=21(0,ω)⊗q）

如果角速度 ω \omega ω 在惯性系表示，或你采用左乘约定，则线性化形式与上面唯一不同之处在于 叉乘块的符号。重复同样步骤可得到：

1. 连续线性误差动力学（选用合适的误差定义）

若采用相应的误差定义（例如把误差定义为左乘   q = δ q ⊗ q ^ \, q = \delta q \otimes \hat q q=δq⊗q^）并线性化，结果为：

δ ϕ ˙ = + [ ω m − b ^ ] ×   δ ϕ    −    δ b    −    n g , δ b ˙ = n b g . \boxed{% \begin{aligned} \delta\dot\phi &= + [\omega_m - \hat b]\times \, \delta\phi \;-\; \delta b \;-\; n_g,\\ \delta\dot b &= n{bg}. \end{aligned} } δϕ˙δb˙=+[ωm−b^]×δϕ−δb−ng,=nbg.

注意：和右乘相比， [ ω ] [\omega] [ω] 前没有负号 ------ 也就是 A A A 的左上块符号反了。

相应的 A A A 与 B B B 为：

A inertial = ( [ ω m − b ^ ] ×    − I 3 0 3 × 3    0 3 × 3 ) , B inertial = ( I 3 0 3 0 I 3 ) . \boxed{% A_{\text{inertial}} = \begin{pmatrix} [\omega_m - \hat b]\times &\; -I{3} \\ 0_{3\times3} &\; 0_{3\times3} \end{pmatrix}, \quad B_{\text{inertial}} = \begin{pmatrix} I_3 & 0_{3} \\ 0 & I_3 \end{pmatrix}. } Ainertial=([ωm−b^]×03×3−I303×3),Binertial=(I3003I3).

2. 离散化 / Φ \Phi Φ（闭式）

同样利用块上三角矩阵指数形式：

Φ inertial ( Δ t ) = ( exp ⁡ ( [ ω ] Δ t )    − Φ ~ ϕ b 0 I 3 ) , \Phi_{\text{inertial}}(\Delta t) = \begin{pmatrix} \exp( [\omega]\Delta t) &\; -\tilde\Phi_{\phi b} \\ 0 & I_3 \end{pmatrix}, Φinertial(Δt)=(exp([ω]Δt)0−Φ~ϕbI3),

其中

Φ ~ ϕ b = ∫ 0 Δ t exp ⁡ ( [ ω ] ( Δ t − s ) )   d s = [ ω ] − 1 ( exp ⁡ ( [ ω ] Δ t ) − I ) . \tilde\Phi_{\phi b} = \int_0^{\Delta t} \exp( [\omega] (\Delta t - s)) \, ds = [\omega]^{-1} ( \exp([\omega]\Delta t) - I ). Φ~ϕb=∫0Δtexp([ω](Δt−s))ds=[ω]−1(exp([ω]Δt)−I).

（注意与 body-case 符号的差别：body-case 使用 exp ⁡ ( − [ ω ] Δ t ) \exp(-[\omega]\Delta t) exp(−[ω]Δt) 且 Φ ϕ b = − Ω − 1 ( I − e Ω Δ t ) \Phi_{\phi b} = -\Omega^{-1}(I - e^{\Omega\Delta t}) Φϕb=−Ω−1(I−eΩΔt)；inertial-case 是对应的正号版本。）

小角近似也相应替换符号。

---

III. 汇总对比

• 右乘（body）线性化 A A A 的左上块： − [ ω m − b ^ ] × -[\omega_m-\hat b]\times −[ωm−b^]×。离散主块： Φ ϕ ϕ = exp ⁡ ( − [ ω ] Δ t ) \Phi{\phi\phi} = \exp(-[\omega]\Delta t) Φϕϕ=exp(−[ω]Δt)。
• 左乘（inertial）线性化 A A A 的左上块： + [ ω m − b ^ ] × +[\omega_m-\hat b]\times +[ωm−b^]×。离散主块： Φ ϕ ϕ = exp ⁡ ( + [ ω ] Δ t ) \Phi{\phi\phi} = \exp(+[\omega]\Delta t) Φϕϕ=exp(+[ω]Δt)。
• 两者 B B B（噪声映射）在形式上相同（陀螺噪声以负号进入 δ ϕ ˙ \delta\dot\phi δϕ˙，偏置噪声为随机游走）。

---

IV. 基于Eigen库的代码示例

下面给出右乘 / body-frame 的常用实现片段（包括计算 Φ \Phi Φ 的闭式块并给出小角近似以防 ∥ ω ∥ \|\omega\| ∥ω∥ 很小）：

#include <Eigen/Dense>
using Vec3 = Eigen::Vector3d;
using Mat3 = Eigen::Matrix3d;
using Mat6 = Eigen::Matrix<double,6,6>;
using Mat63 = Eigen::Matrix<double,6,3>;

// compute skew-symmetric
Mat3 skew(const Vec3 &v) {
    Mat3 S;
    S <<    0, -v.z(),  v.y(),
         v.z(),     0, -v.x(),
        -v.y(),  v.x(),     0;
    return S;
}

// compute Phi for right-multiplicative / body-frame
void computePhiBody(const Vec3 &omega_hat, double dt,
                    Eigen::Matrix<double,6,6> &Phi)
{
    Mat3 Omega = skew(omega_hat); // [omega]_x
    // compute exp(-Omega*dt) via Rodrigues
    double theta = omega_hat.norm() * dt;
    Mat3 R;
    if (theta < 1e-8) {
        R = Mat3::Identity() - Omega * dt; // first-order
    } else {
        Vec3 u = omega_hat.normalized();
        Mat3 K = skew(u);
        R = Mat3::Identity()
            - sin(theta) * K
            + (1 - cos(theta)) * K * K;
        // note: exp(-[omega]*dt) = R^T if R=exp([omega]*dt) depending sign,
        // here we used formula for exp(-Omega*dt) by replacing theta->theta and sign.
        // one must be consistent: use exp(-Omega*dt) implementation.
    }

    // top-right block: Phi_phi_b = - Omega^{-1} (I - exp(-Omega dt))
    Mat3 Phi_phiphi = R;
    Mat3 Phi_phib;
    if (omega_hat.norm() < 1e-8) {
        // Taylor series
        Phi_phib = -Mat3::Identity() * dt + 0.5 * Omega * dt * dt;
    } else {
        Mat3 Omega_inv = Omega.inverse(); // safe because Omega is invertible for non-zero omega
        Phi_phib = - Omega_inv * (Mat3::Identity() - R);
    }

    Phi.setZero();
    Phi.block<3,3>(0,0) = Phi_phiphi;
    Phi.block<3,3>(0,3) = Phi_phib;
    Phi.block<3,3>(3,3) = Mat3::Identity();
}

关于 Q d Q_d Qd，可使用数值积分或近似公式。一个简单且常用的离散过程噪声近似（工程实现）：

// Qc = diag(Qg, Qbg), each 3x3
// approximate Qd:
void approxProcessNoise(const Mat3 &Qg, const Mat3 &Qbg, double dt, Eigen::Matrix<double,6,6> &Qd)
{
    Qd.setZero();
    // top-left: gyro noise integrated
    Qd.block<3,3>(0,0) = Qg * dt + Mat3::Zero(); // more accurate has extra terms
    Qd.block<3,3>(0,3) = Mat3::Zero(); // often small
    Qd.block<3,3>(3,0) = Mat3::Zero();
    Qd.block<3,3>(3,3) = Qbg * dt;
    // for better accuracy include cross terms from integral expression
}

若需要更精确 Q d Q_d Qd，请用解析积分（按 Φ \Phi Φ 的闭式块）或用数值求解（高精度但稍慢）。

---

V. 总结

1. 先选约定 ------ 明确自己用的是右乘(body)还是左乘(inertial)，以及四元数分量顺序（scalar-first/last）。混淆会造成 ± 符号错误。
2. 误差态：误差态通常用 3 元小角 + 偏置（6 维）来替代 4 元四元数直接线性化（避免单位约束）。
3. 连续线性化 ：结果是块上三角矩阵，左上块为 ± [ ω ] × \pm[\omega]_\times ±[ω]×（符号取决于乘法约定），左上右列为 − I -I −I，下行全零。
4. 离散化 ： Φ \Phi Φ 的主块为旋转矩阵 exp ⁡ ( ± [ ω ] Δ t ) \exp(\pm [\omega]\Delta t) exp(±[ω]Δt)。 Φ ϕ b \Phi_{\phi b} Φϕb 有闭式表达 − Ω − 1 ( I − e Ω Δ t ) -\Omega^{-1}(I - e^{\Omega \Delta t}) −Ω−1(I−eΩΔt)（body-case），或其符号反转（inertial-case）。
5. 噪声传播 ： Q d Q_d Qd 可用解析积分或数值积分得到；工程上常用简单近似或查表公式（若需要精确，请用闭式积分或数值评估）。

---