对 Error-State Kalman Filter 的理解

我对 Error-State Kalman Filter 的理解。本文的主要参考文献是 Joan Sola 的 《Quaternion kinematics for the error-state Kalman filter》[1]，当然是这本小册子，做 VIO 的人都会熟读这本册子。

1. ErKF 与 EFK 的区别

关于 Error-State Kalman Filter (ErKF) 与 Extended Kalman Filter (EKF) 之间的联系与区别可以参考论文 [2]，论文 [2] 可以帮助理解 Error-State Kalman Filter，现在引用其中的一些文字以说明 EKF 与 ErKF 之间的区别。

论文 [2] 的 Abstract：

EKFs operate by linearizing the nonlinear model around the current reference trajectory and then designing the Kalman filter gain for the linearized model. Recently, an alternative approach has emerged for a certain class of problems where the error in the states is estimated using a Kalman filter, rather than the state itself.

EKF 直接估计状态值，而 ErKF 估计的是状态值的误差部分。

论文 [2] Remark II.6. 的最后一句：

Thus, this matrix is a linearized matrix,unlike in the ErKF case, which is a linear matrix.

上文中的 this matrix 是指 "the system matrix, (mathbf{F}(t)), in EKF formulation"。ErKF 的 system matrix 在论文 [2] 中的

论文 [2] 中 EKF 与 ErKF 的 system matrix (mathbf{F}(t)) 分别是公式 (11) 与 (18)。

1.1. EKF 的近似

EKF 的 (mathbf{F}(t)) 用于对 States 的 Covariance 估计，可以将公式 (10) (11) (12) 结合一起看。

在 (t) 时刻 States 的 Covariance 是 (mathbf{P}(t) = mathbf{E}[(mathbf{x}(t) - hat{mathbf{x}}(t))(mathbf{x}(t) - hat{mathbf{x}}(t))^T])，其中 (mathbf{x}(t)) 是 States 真值，(hat{mathbf{x}}(t)) 是系统对 States 的的估计值。经过时间 (Delta t) 到达下一个时刻 (t + Delta t)，此时 Covariance 可以如下表示。

[egin{align} mathbf{P}(t + Delta t) = mathbf{E}[(mathbf{x}(t + Delta t) - hat{mathbf{x}}(t + Delta t))(mathbf{x}(t + Delta t) - hat{mathbf{x}}(t + Delta t))^T] end{align}]

但是在没有观测之前，我们尚未获得系统在 (t + Delta t) 时刻的结果，即估计值 (hat{mathbf{x}}(t + Delta t))。考虑在 (t + Delta t) 时刻没有观测之前的系统 Covariance (mathbf{P}^-(t + Delta t))。需要将上面公式中的 (hat{mathbf{x}}(t + Delta t)) 替换为 (hat{mathbf{x}}^-(t + Delta t))，表示是用 (hat{mathbf{x}}(t)) 经过运动方程计算得到的 (t + Delta t) 时刻的 Predict 值（之后还需要经过 Update 过程修正）。

[egin{align} hat{mathbf{x}}^-(t + Delta t) &= hat{mathbf{x}}(t) + dot{hat{mathbf{x}}}(t) Delta t otag \ &= hat{mathbf{x}}(t) + (mathbf{f}(hat{mathbf{x}}(t), mathbf{u}(t)) + mathbf{Gamma}mathbf{w}(t)) Delta t \ mathbf{P}^-(t + Delta t) &= mathbf{E}[(mathbf{x}(t + Delta t) - hat{mathbf{x}}^-(t + Delta t))(mathbf{x}(t + Delta t) - hat{mathbf{x}}^-(t + Delta t))^T] end{align}]

对于 (t + Delta t) 时刻的真值，系统是无从得知的，使用 (t) 时刻的真值与运动方程估计。

[egin{align} mathbf{x}^-(t + Delta t) &= mathbf{x}(t) + dot{mathbf{x}}(t) Delta t otag \ &= mathbf{x}(t) + [mathbf{f}(mathbf{x}(t), mathbf{u}(t)) + mathbf{Gamma}mathbf{w}(t)] Delta t otag \ &simeq mathbf{x}(t) + left[ mathbf{f}(hat{mathbf{x}}(t), mathbf{u}(t)) + {partial mathbf{f}(hat{mathbf{x}}(t), mathbf{u}(t)) over partial mathbf{x}(t)}(mathbf{x}(t) - hat{mathbf{x}}(t)) + mathbf{Gamma}mathbf{w}(t) ight] Delta t （泰勒一阶展开近似） otag \ &= mathbf{x}(t) + mathbf{f}(hat{mathbf{x}}(t), mathbf{u}(t)) Delta t + mathbf{F}(t) (mathbf{x}(t) - hat{mathbf{x}}(t)) Delta t + mathbf{Gamma}mathbf{w}(t) Delta t end{align}]

计算 (mathbf{P}^-(t + Delta t))。

[egin{align} mathbf{P}^-(t + Delta t) &= mathbf{E}[(mathbf{x}^-(t + Delta t) - hat{mathbf{x}}^-(t + Delta t))(mathbf{x}^-(t + Delta t) - hat{mathbf{x}}^-(t + Delta t))^T] otag \ &simeq mathbf{E}[(mathbf{x}(t) + mathbf{f}(hat{mathbf{x}}(t), mathbf{u}(t)) Delta t + mathbf{F}(t) (mathbf{x}(t) - hat{mathbf{x}}(t)) Delta t + mathbf{Gamma}mathbf{w}(t) Delta t - hat{mathbf{x}}(t) - mathbf{f}(hat{mathbf{x}}(t), mathbf{u}(t)) Delta t) otag \ &phantom{=}(mathbf{x}(t) + mathbf{f}(hat{mathbf{x}}(t), mathbf{u}(t)) Delta t + mathbf{F}(t) (mathbf{x}(t) - hat{mathbf{x}}(t)) Delta t + mathbf{Gamma}mathbf{w}(t) Delta t - hat{mathbf{x}}(t) - mathbf{f}(hat{mathbf{x}}(t), mathbf{u}(t)) Delta t)^T ] otag \ &= mathbf{E}[((mathbf{I} + mathbf{F}(t)Delta t)(mathbf{x}(t) - hat{mathbf{x}}(t))) ((mathbf{I} + mathbf{F}(t)Delta t)(mathbf{x}(t) - hat{mathbf{x}}(t)))^T] otag \ &phantom{=} mathbf{E}[mathbf{Gamma}mathbf{w}(t)((mathbf{I} + mathbf{F}(t)Delta t)(mathbf{x}(t) - hat{mathbf{x}}(t)))^T] + mathbf{E}[((mathbf{I} + mathbf{F}(t)Delta t)(mathbf{x}(t) - hat{mathbf{x}}(t)))mathbf{w}(t)^Tmathbf{Gamma}^T] otag \ &phantom{=} + mathbf{E}[mathbf{Gamma}mathbf{w}(t)mathbf{w}(t)^Tmathbf{Gamma}^T] otag \ &= (mathbf{I} + mathbf{F}(t)Delta t) mathbf{E}[(mathbf{x}(t) - hat{mathbf{x}}(t)) (mathbf{x}(t) - hat{mathbf{x}}(t))^T] (mathbf{I} + mathbf{F}(t)Delta t)^T otag \ &phantom{=} mathbf{Gamma}mathbf{E}[mathbf{w}(t)]mathbf{E}[((mathbf{I} + mathbf{F}(t)Delta t)(mathbf{x}(t) - hat{mathbf{x}}(t)))^T] + mathbf{E}[((mathbf{I} + mathbf{F}(t)Delta t)(mathbf{x}(t) - hat{mathbf{x}}(t)))]mathbf{E}[mathbf{w}(t)^T]mathbf{Gamma}^T otag \ &phantom{=} + mathbf{Gamma}mathbf{E}[mathbf{w}(t)mathbf{w}(t)^T]mathbf{Gamma}^T otag \ &= (mathbf{I} + mathbf{F}(t)Delta t) mathbf{P}(t) (mathbf{I} + mathbf{F}(t)Delta t)^T + mathbf{Gamma}mathbf{Q}mathbf{Gamma}^T phantom{=} ( ext{We have } mathbf{E}[mathbf{w}(t)] = 0) otag \ &= mathbf{P}(t) + mathbf{P}(t)mathbf{F}(t)^TDelta t + mathbf{F}(t) mathbf{P}(t) Delta t + mathbf{F}(t) mathbf{P}(t)mathbf{F}(t)^TDelta t^2 + mathbf{Gamma}mathbf{Q}mathbf{Gamma}^T otag \ &{simeq} mathbf{P}(t) + mathbf{P}(t)mathbf{F}(t)^TDelta t + mathbf{F}(t) mathbf{P}(t) Delta t + mathbf{Gamma}mathbf{Q}mathbf{Gamma}^T phantom{=} (Delta t^2 ext{ too small, ignore second order and higher}) end{align}]

所以得到 (mathbf{P}(t)) 的微分方程。

[egin{align} dot{mathbf{P}}(t) &= mathbf{P}(t)mathbf{F}(t)^T + mathbf{F}(t) mathbf{P}(t) + mathbf{Gamma}mathbf{Q}mathbf{Gamma}^T end{align}]

与公式 (11) 一致。所以，得到关于 EKF 近似的结论，EKF 在 Predict 步骤中使用上一时刻对 States 的估计值 (hat{mathbf{x}}(t))，对运动方程 (mathbf{f}) 使用一阶泰勒展开近似下一时刻 States 的真值 (mathbf{x}(t + Delta t))。从而估计 Predict 步骤得到的 States Covariance。

1.2. ErKF 无近似

ErKF 估计的是 States 的误差部分。

在 Predict 步骤的操作如下。

省略了控制值与误差之后的微分方程如下。（论文中的符号混乱，所以我重新整理。）

[egin{align} dot{mathbf{x}}(t) &= mathbf{f}(mathbf{x}(t), mathbf{u}(t)) + mathbf{Gamma}mathbf{w}(t) end{align}]

将真值分解成为 nominal 和 error 两个部分。nomianl 部分使用 (hat{mathbf{x}}(t)) 表示，error 部分使用 (delta mathbf{x}(t)) 表示。真值 (mathbf{x}(t) = hat{mathbf{x}}(t) + delta mathbf{x}(t)) 。代入运动（微分）方程。

[egin{align} dot{hat{mathbf{x}}}(t) + deltadot{mathbf{x}}(t) &= mathbf{f}(hat{mathbf{x}}(t)+deltamathbf{x}(t),mathbf{u}(t)) + mathbf{Gamma}mathbf{w}(t) otag \ &= mathbf{f}(hat{mathbf{x}}(t),mathbf{u}(t)) + {partial mathbf{f}(hat{mathbf{x}}(t),mathbf{u}(t)) over partial mathbf{x}(t)} deltamathbf{x}(t) otag \ &phantom{=} + {1 over 2!}{partial^2 mathbf{f}(hat{mathbf{x}}(t),mathbf{u}(t)) over partial mathbf{x}(t)^2} deltamathbf{x}(t)^2 + dots + mathbf{Gamma}mathbf{w}(t) otag \ &= mathbf{f}(hat{mathbf{x}}(t),mathbf{u}(t)) + mathbf{F}(t) deltamathbf{x}(t) + O(deltamathbf{x}(t)^2) + mathbf{Gamma}mathbf{w}(t) \ mathbf{F}(t) &= {partial mathbf{f}(hat{mathbf{x}}(t),mathbf{u}(t)) over partial mathbf{x}(t)} \ dot{hat{mathbf{x}}}(t) &= mathbf{f}(hat{mathbf{x}}(t),mathbf{u}(t)) phantom{=}(定义) \ deltadot{mathbf{x}}(t) &= mathbf{F}(t) deltamathbf{x}(t) + O(deltamathbf{x}(t)^2) + mathbf{Gamma}mathbf{w}(t) end{align}]

由定义可以得到时间 (Delta t) 之后时刻 (t + Delta t) 的 (deltamathbf{x}(t + Delta t))。

[egin{align} deltamathbf{x}(t + Delta t) &= deltamathbf{x}(t) + deltadot{mathbf{x}}(t) Delta t otag \ &= deltamathbf{x}(t) + mathbf{F}(t) deltamathbf{x}(t)Delta t otag \ &phantom{=} + O(deltamathbf{x}(t)^2) Delta t + mathbf{Gamma}mathbf{w}(t)Delta t otag \ &= (mathbf{I} + mathbf{F}(t)Delta t)deltamathbf{x}(t) otag \ &phantom{=} + O(deltamathbf{x}(t)^2) Delta t + mathbf{Gamma}mathbf{w}(t)Delta t otag \ &= mathbf{Phi}(t)deltamathbf{x}(t) + O(deltamathbf{x}(t)^2) Delta t + mathbf{Gamma}mathbf{w}(t)Delta t end{align}]

在 ErKF 中 (mathbf{Phi}(t)) 被称作 State Transition Matrix。ErKF 的 Predict 步骤做近似 (deltamathbf{x}(t + Delta t) simeq mathbf{Phi}(t)deltamathbf{x}(t))，剩下的 (O(deltamathbf{x}(t)^2) Delta t + mathbf{Gamma}mathbf{w}(t)Delta t) 在 Update 步骤用观测方程进行估计。

从 (mathbf{F}(t)) 的定义中可以看到，其与 error (deltamathbf{x}(t)) 是无关的，使用上一步的估计值 (hat{mathbf{x}}(t)) 与这一步的控制值 (mathbf{u}(t)) 即可计算得到。于是，在 Predict 步骤无需设计 error 部分 (deltamathbf{x}(t)) 的 error，对 (mathbf{Phi}(t)) 进行近似。

所以，可以得到结论，ErKF 的 Predict 步骤是一个线性的步骤。存在的近似 (O(deltamathbf{x}(t)^2) Delta t + mathbf{Gamma}mathbf{w}(t)Delta t) 可以在 Update 步骤估计出来。

2. 应用 ErKF 至 VIO

参考 [1] 进行此部分的推导。从博客 《Rotation Kinematics》 的 Quaternion 微分方程开始。

事先声明，我使用 Hamilton Quaternion。

先定义系统的状态向量。

[egin{align} mathbf{x} &= egin{bmatrix} {}^Gmathbf{q}^T_I & mathbf{b}_g^T & {}^Gmathbf{v}_I^T & mathbf{b}_a^T & {}^Gmathbf{p}_I^T | {}^Gmathbf{f}_1^T & dots & {}^Gmathbf{f}_N^T end{bmatrix}^T \ &= egin{bmatrix} mathbf{x}_s^T & | & mathbf{x}_f^T end{bmatrix}^T end{align}]

(I) 指 IMU 坐标系，在常规情况下 VIO 系统的 Body 坐标系与 IMU 坐标系重合，(G) 指 Global 坐标系，即 Inertial Frame。各个状态值的含义如下：

1. ({}^Gmathbf{q}_I) 将点在 (I) 坐标系下的坐标转换到 (G) 坐标系下的坐标所使用的四元数；
2. (mathbf{b}_g) 陀螺仪测量值的 bias；
3. ({}^Gmathbf{v}_I) 在 (I) 坐标系原点在 (G) 坐标系下坐标的速度，即 ({}^Gdot{mathbf{p}}_I)；
4. (mathbf{b}_a) 加速度计测量值的 bias；
5. ({}^Gmathbf{p}_I) 将点在 (I) 坐标系下的坐标转换到 (G) 坐标系下的坐标所使用的偏移，即 (I) 坐标系原点在 (G) 坐标系下的坐标；
6. ({}^Gmathbf{f}_1^T) 是指地标点 1 在 (G) 坐标系下的坐标。

有 Transform 过程。

[egin{align} {}^Gmathbf{f}_i &= {}^Gmathbf{q}_I{}^Imathbf{f}_i + {}^Gmathbf{p}_I, &i = 1, dots, N end{align}]

系统的运动（微分）方程，可以看做是在 IMU 测量值的线加速度、角速度作用下，系统以上的状态值发生变化。

博客 《Rotation Kinematics》 给出的 Quatenrion 微分方程如下。

[egin{align} {}^Gdot{mathbf{q}}_I &= {1 over 2} {}^Gmathbf{q}_I otimes mathbf{omega}^{IG}_I(t) end{align}]

系统中所有状态值（真值）的微分方程写成如下形式。

[egin{align} {}^Gdot{mathbf{q}}_I(t) &= {1 over 2} {}^Gmathbf{q}_I(t) otimes mathbf{omega}^{IG}_I(t) \ dot{mathbf{b}}_g &= mathbf{n}_{wg}(t) \ {}^Gdot{mathbf{v}}_I(t) &= {}^Gmathbf{a}(t) \ dot{mathbf{b}}_a(t) &= mathbf{n}_{wa}(t) \ {}^Gdot{mathbf{p}}_I(t) &= {}^Gmathbf{v}_I(t) \ {}^Gdot{mathbf{f}}_i(t) &= mathbf{0}_{3 imes 1}, phantom{ } phantom{ } phantom{ } i = 1, dots, N end{align}]

系统的输入值角速度与加速的测量值存在误差，会将误差引入到运动方程中。依次将状态值的真值划分成两个部分—— nominal 与 error 。

2.1. 运动微分方程的 nominal 与 error

角速度与加速度的测量值与真值的关系如下。

[egin{align} mathbf{omega}_m(t) &= mathbf{omega}(t) + mathbf{b}_g(t) + mathbf{n}_g(t) \ mathbf{a}_m(t) &= mathbf{C}({}^Imathbf{q}_G(t))({}^Gmathbf{a}(t) - {}^Gmathbf{g}) + mathbf{b}_a(t) + mathbf{n}_a(t) end{align}]

将角速度与加速度的真值区分为 nominal 与 error 两个部分。nominal 部分用上标 (hat{cdot}) 表示，error 用上标 ( ilde{cdot}) 表示。

[egin{align} mathbf{omega}(t) &= (mathbf{omega}_m(t) - mathbf{b}_g(t)) - mathbf{n}_g(t) otag \ &= (mathbf{omega}_m(t) - hat{mathbf{b}}_g(t) + ilde{mathbf{b}}_g(t)) - mathbf{n}_g(t) otag \ &= (mathbf{omega}_m(t) - hat{mathbf{b}}_g(t)) - (mathbf{n}_g(t) - ilde{mathbf{b}}_g(t)) otag \ &= hat{mathbf{omega}}(t) - ilde{mathbf{omega}}(t) \ {}^Gmathbf{a}(t) &= mathbf{C}^T({}^Gmathbf{q}_I(t))left[ (mathbf{a}_m(t) - hat{mathbf{b}}_a(t)) - (mathbf{n}_a(t) - ilde{mathbf{b}}_a(t)) ight] + {}^Gmathbf{g} otag \ &= mathbf{C}^T({}^Gmathbf{q}_I(t))left[ hat{mathbf{a}}(t) - ilde{mathbf{a}}(t) ight] + {}^Gmathbf{g} otag \ &= mathbf{C}^T({}^Ghat{mathbf{q}}_I(t) otimes {}^G ilde{mathbf{q}}_I(t))left[ hat{mathbf{a}}(t) - ilde{mathbf{a}}(t) ight] + {}^Gmathbf{g} otag \ &= mathbf{C}^Tleft({}^Ghat{mathbf{q}}_I(t) otimes egin{bmatrix} 1 \ {1 over 2}deltamathbf{ heta}(t) end{bmatrix} ight)left[ hat{mathbf{a}}(t) - ilde{mathbf{a}}(t) ight] + {}^Gmathbf{g} otag \ &= mathbf{C}^Tleft(mathbf{C}^{-1}left(mathbf{C}left({}^Ghat{mathbf{q}}_I(t) otimes egin{bmatrix} 1 \ {1 over 2}deltamathbf{ heta}(t) end{bmatrix} ight) ight) ight)left[ hat{mathbf{a}}(t) - ilde{mathbf{a}}(t) ight] + {}^Gmathbf{g} otag \ &= mathbf{C}^Tleft(mathbf{C}^{-1}left(mathbf{C}[{}^Ghat{mathbf{q}}_I(t)] ext{Exp}[deltamathbf{ heta}(t)] ight) ight)left[ hat{mathbf{a}}(t) - ilde{mathbf{a}}(t) ight] + {}^Gmathbf{g} otag \ &= {mathbf{C}left(mathbf{C}^{-1}left(mathbf{C}[{}^Ghat{mathbf{q}}_I(t)] ext{Exp}[deltamathbf{ heta}(t)] ight) ight)}^Tleft[ hat{mathbf{a}}(t) - ilde{mathbf{a}}(t) ight] + {}^Gmathbf{g} otag \ &= {mathbf{C}[{}^Ghat{mathbf{q}}_I(t)] ext{Exp}[deltamathbf{ heta}(t)]}^Tleft[ hat{mathbf{a}}(t) - ilde{mathbf{a}}(t) ight] + {}^Gmathbf{g} otag \ &= ext{Exp}[-deltamathbf{ heta}(t)]mathbf{C}^T[{}^Ghat{mathbf{q}}_I(t)]left[ hat{mathbf{a}}(t) - ilde{mathbf{a}}(t) ight] + {}^Gmathbf{g} otag \ &= [mathbf{I}-deltamathbf{ heta}(t)^{wedge}]mathbf{C}^T[{}^Ghat{mathbf{q}}_I(t)]left[ hat{mathbf{a}}(t) - ilde{mathbf{a}}(t) ight] + {}^Gmathbf{g} otag \ &= mathbf{C}^T({}^Ghat{mathbf{q}}_I(t))left[ hat{mathbf{a}}(t) - ilde{mathbf{a}}(t) ight]-deltamathbf{ heta}(t)^{wedge}mathbf{C}^T({}^Ghat{mathbf{q}}_I(t))left[ hat{mathbf{a}}(t) - ilde{mathbf{a}}(t) ight] + {}^Gmathbf{g} end{align}]

代入状态值的真值中，将所有状态值的 nominal 与 error 分离，分别写出它们的微分方程。

先做较为复杂的姿态四元数部分。定义四元数真值与其 nominal、error 的关系如下。

[egin{align} {}^Gmathbf{q}_I(t) &= {}^Ghat{mathbf{q}}_I(t) otimes {}^G ilde{mathbf{q}}_I(t) \ {}^G ilde{mathbf{q}}_I(t) &= egin{bmatrix} 1 \ {1 over 2}deltamathbf{ heta}(t) end{bmatrix} end{align}]

即四元数的 error 部分对应的是一个小角度的旋转，可以用轴角 (deltamathbf{ heta}(t)) 表示。

直接定义四元数 nominal 部分的微分方程为 ({}^Gdot{hat{mathbf{q}}}_I(t) = {1 over 2} {}^Ghat{mathbf{q}}_I(t) otimes hat{mathbf{omega}}(t))。

下面的这个公式推导参考论文 [1] 的公式 (259) 附近的推导。

[egin{align} {}^Gdot{mathbf{q}}_I(t) &= {}^Gdot{hat{mathbf{q}}}_I(t) otimes {}^G ilde{mathbf{q}}_I(t) + {}^Ghat{mathbf{q}}_I(t) otimes {}^Gdot{ ilde{mathbf{q}}}_I(t) otag \ {1 over 2} {}^Gmathbf{q}_I(t) otimes mathbf{omega}(t) &= {}^Gdot{hat{mathbf{q}}}_I(t) otimes {}^G ilde{mathbf{q}}_I(t) + {}^Ghat{mathbf{q}}_I(t) otimes {}^Gdot{ ilde{mathbf{q}}}_I(t) otag \ {1 over 2} {}^Ghat{mathbf{q}}_I(t) otimes {}^G ilde{mathbf{q}}_I(t) otimes mathbf{omega}(t) &= {1 over 2} {}^Ghat{mathbf{q}}_I(t) otimes hat{mathbf{omega}}(t) otimes {}^G ilde{mathbf{q}}_I(t) + {}^Ghat{mathbf{q}}_I(t) otimes {}^Gdot{ ilde{mathbf{q}}}_I(t) otag \ {1 over 2} {}^G ilde{mathbf{q}}_I(t) otimes mathbf{omega}(t) &= {1 over 2} hat{mathbf{omega}}(t) otimes {}^G ilde{mathbf{q}}_I(t) + {}^Gdot{ ilde{mathbf{q}}}_I(t) otag \ 2 {}^Gdot{ ilde{mathbf{q}}}_I(t) &= {}^G ilde{mathbf{q}}_I(t) otimes mathbf{omega}(t) - hat{mathbf{omega}}(t) otimes {}^G ilde{mathbf{q}}_I(t) otag \ 2 egin{bmatrix} 0 \ {1 over 2}dot{deltamathbf{ heta}}(t) end{bmatrix} &= {}^G ilde{mathbf{q}}_I(t) otimes mathbf{omega}(t) - hat{mathbf{omega}}(t) otimes {}^G ilde{mathbf{q}}_I(t) otag \ egin{bmatrix} 0 \ dot{deltamathbf{ heta}}(t) end{bmatrix} &= [mathbf{omega}(t)]_R {}^G ilde{mathbf{q}}_I(t) - [hat{mathbf{omega}}(t)]_L {}^G ilde{mathbf{q}}_I(t) otag \ &= egin{bmatrix} 0 & -mathbf{omega}(t)^T \ mathbf{omega}(t) & -mathbf{omega}(t)^{wedge} end{bmatrix} {}^G ilde{mathbf{q}}_I(t) - egin{bmatrix} 0 & -hat{mathbf{omega}}(t)^T \ hat{mathbf{omega}}(t) & hat{mathbf{omega}}(t)^{wedge} end{bmatrix} {}^G ilde{mathbf{q}}_I(t) otag \ &= egin{bmatrix} 0 & -(mathbf{omega}(t)-hat{mathbf{omega}}(t))^T \ mathbf{omega}(t)-hat{mathbf{omega}}(t) & -(mathbf{omega}(t)+hat{mathbf{omega}}(t))^{wedge} end{bmatrix} egin{bmatrix} 1 \ {1 over 2}deltamathbf{ heta}(t) end{bmatrix} end{align}]

所以可以得四元数 error 部分的微分方程。

[egin{align} dot{deltamathbf{ heta}}(t) &= mathbf{omega}(t)-hat{mathbf{omega}}(t) - {1 over 2}(mathbf{omega}(t)+hat{mathbf{omega}}(t))^{wedge} deltamathbf{ heta}(t) otag \ &= - ilde{mathbf{omega}}(t) - {1 over 2}(2hat{mathbf{omega}}(t) - ilde{mathbf{omega}}(t))^{wedge} deltamathbf{ heta}(t) otag \ &= - ilde{mathbf{omega}}(t) - hat{mathbf{omega}}(t)^{wedge}deltamathbf{ heta}(t) + {1 over 2} ilde{mathbf{omega}}(t)^{wedge}deltamathbf{ heta}(t) otag \ &simeq -hat{mathbf{omega}}(t)^{wedge}deltamathbf{ heta}(t) - ilde{mathbf{omega}}(t) phantom{=}(二阶小量近似 0) end{align}]

其他状态值的 nominal 与 error 分离如下。

[egin{align} dot{mathbf{b}}_g(t) &= mathbf{n}_{wg}(t) otag \ &= mathbf{0}_3 - (-mathbf{n}_{wg}(t)) otag \ &= dot{hat{mathbf{b}}}_g(t) - dot{ ilde{mathbf{b}}}_g(t) \ {}^Gdot{mathbf{v}}_I(t) &= {}^Gmathbf{a}(t) otag \ &= mathbf{C}^T({}^Ghat{mathbf{q}}_I(t))left[ hat{mathbf{a}}(t) - ilde{mathbf{a}}(t) ight]-deltamathbf{ heta}(t)^{wedge}mathbf{C}^T({}^Ghat{mathbf{q}}_I(t))left[ hat{mathbf{a}}(t) - ilde{mathbf{a}}(t) ight] + {}^Gmathbf{g} otag \ &= mathbf{C}^T({}^Ghat{mathbf{q}}_I(t))hat{mathbf{a}}(t) + {}^Gmathbf{g} - mathbf{C}^T({}^Ghat{mathbf{q}}_I(t)) ilde{mathbf{a}}(t) - deltamathbf{ heta}(t)^{wedge}mathbf{C}^T({}^Ghat{mathbf{q}}_I(t))hat{mathbf{a}}(t) - deltamathbf{ heta}(t)^{wedge}mathbf{C}^T({}^Ghat{mathbf{q}}_I(t)) ilde{mathbf{a}}(t) otag \ &simeq (mathbf{C}^T({}^Ghat{mathbf{q}}_I(t))hat{mathbf{a}}(t) + {}^Gmathbf{g}) - mathbf{C}^T({}^Ghat{mathbf{q}}_I(t)) ilde{mathbf{a}}(t) - deltamathbf{ heta}(t)^{wedge}mathbf{C}^T({}^Ghat{mathbf{q}}_I(t))hat{mathbf{a}}(t) otag \ &= (mathbf{C}^T({}^Ghat{mathbf{q}}_I(t))hat{mathbf{a}}(t) + {}^Gmathbf{g}) - [mathbf{C}^T({}^Ghat{mathbf{q}}_I(t)) ilde{mathbf{a}}(t) - deltamathbf{ heta}(t)^{wedge}mathbf{C}^T({}^Ghat{mathbf{q}}_I(t))hat{mathbf{a}}(t)] otag \ &= (mathbf{C}^T({}^Ghat{mathbf{q}}_I(t))hat{mathbf{a}}(t) + {}^Gmathbf{g}) - {mathbf{C}^T({}^Ghat{mathbf{q}}_I(t)) ilde{mathbf{a}}(t) + [mathbf{C}^T({}^Ghat{mathbf{q}}_I(t))hat{mathbf{a}}(t)]^{wedge}deltamathbf{ heta}(t)} otag \ &= {}^Gdot{hat{mathbf{v}}}_I(t) - {}^Gdot{ ilde{mathbf{v}}}_I(t) \ dot{mathbf{b}}_a(t) &= mathbf{n}_{wa}(t) otag \ &= mathbf{0}_3 - (-mathbf{n}_{wa}(t)) otag \ &= dot{hat{mathbf{b}}}_a(t) - dot{ ilde{mathbf{b}}}_a(t) \ {}^Gdot{mathbf{p}}_I(t) &= {}^Gmathbf{v}_I(t) otag \ &= {}^Ghat{mathbf{v}}_I(t) - {}^G ilde{mathbf{v}}_I(t) otag \ &= {}^Gdot{hat{mathbf{p}}}_I(t) - {}^Gdot{ ilde{mathbf{p}}}_I(t) \ {}^Gdot{mathbf{f}}_i(t) &= mathbf{0}_{3 imes 1}, phantom{ } phantom{ } phantom{ } i = 1, dots, N end{align}]

整理。

nominal 部分有以下运动微分方程。

[egin{align} {}^Gdot{hat{mathbf{q}}}_I(t) &= {1 over 2} {}^Ghat{mathbf{q}}_I(t) otimes hat{mathbf{omega}}(t) \ dot{hat{mathbf{b}}}_g(t) &= mathbf{0}_3 \ {}^Gdot{hat{mathbf{v}}}_I(t) &= mathbf{C}^Tleft({}^Ghat{mathbf{q}}_I(t) ight)hat{mathbf{a}}(t) + {}^Gmathbf{g} \ dot{hat{mathbf{b}}}_a(t) &= mathbf{0}_3 \ {}^Gdot{hat{mathbf{p}}}_I(t) &= {}^Ghat{mathbf{v}}_I(t) \ {}^Gdot{hat{mathbf{f}}}_i(t) &= mathbf{0}_3, phantom{ } phantom{ } phantom{ } i = 1, dots, N end{align}]

error 部分有以下运动微分方程。

[egin{align} dot{deltamathbf{ heta}}(t) &= -hat{mathbf{omega}}(t)^{wedge}deltamathbf{ heta}(t) - ilde{mathbf{omega}}(t) \ &= -hat{mathbf{omega}}(t)^{wedge}deltamathbf{ heta}(t) - mathbf{n}_g(t) + ilde{mathbf{b}}_g(t)\ dot{ ilde{mathbf{b}}}_g(t) &= -mathbf{n}_{wg}(t) \ {}^Gdot{ ilde{mathbf{v}}}_I(t) &= mathbf{C}^T({}^Ghat{mathbf{q}}_I(t)) ilde{mathbf{a}}(t) + [mathbf{C}^T({}^Ghat{mathbf{q}}_I(t))hat{mathbf{a}}(t)]^{wedge}deltamathbf{ heta}(t) otag \ &= mathbf{C}^T({}^Ghat{mathbf{q}}_I(t))(mathbf{n}_a(t) - ilde{mathbf{b}}_a(t)) + [mathbf{C}^T({}^Ghat{mathbf{q}}_I(t))hat{mathbf{a}}(t)]^{wedge}deltamathbf{ heta}(t) otag \ &= mathbf{C}^T({}^Ghat{mathbf{q}}_I(t))mathbf{n}_a(t) - mathbf{C}^T({}^Ghat{mathbf{q}}_I(t)) ilde{mathbf{b}}_a(t) + [mathbf{C}^T({}^Ghat{mathbf{q}}_I(t))hat{mathbf{a}}(t)]^{wedge}deltamathbf{ heta}(t) otag \ &= [mathbf{C}^T({}^Ghat{mathbf{q}}_I(t))hat{mathbf{a}}(t)]^{wedge}deltamathbf{ heta}(t) - mathbf{C}^T({}^Ghat{mathbf{q}}_I(t)) ilde{mathbf{b}}_a(t) + mathbf{C}^T({}^Ghat{mathbf{q}}_I(t))mathbf{n}_a(t) \ dot{ ilde{mathbf{b}}}_a(t) &= -mathbf{n}_{wa}(t) \ {}^Gdot{ ilde{mathbf{p}}}_I(t) &= {}^G ilde{mathbf{v}}_I(t) \ {}^Gdot{ ilde{mathbf{f}}}_i(t) &= mathbf{0}_3, phantom{ } phantom{ } phantom{ } i = 1, dots, N end{align}]

2.2. State Transition Matrix

从以上的微分方程计算 State Transition Matrix。

[egin{align} dot{ ilde{mathbf{x}}}(t) &= egin{bmatrix} dot{deltamathbf{ heta}}(t) \ dot{ ilde{mathbf{b}}}_g(t) \ {}^Gdot{ ilde{mathbf{v}}}_I(t) \ dot{ ilde{mathbf{b}}}_a(t) \ {}^Gdot{ ilde{mathbf{p}}}_I(t) \ {}^Gdot{ ilde{mathbf{f}}}_1 \ vdots \ {}^Gdot{ ilde{mathbf{f}}}_N end{bmatrix} otag \ &= egin{bmatrix} -hat{mathbf{omega}}(t)^{wedge} & mathbf{I}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ [mathbf{C}^Tleft({}^Ghat{mathbf{q}}_I(t) ight)hat{mathbf{a}}(t)]^{wedge} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & -mathbf{C}^T({}^Ghat{mathbf{q}}_I(t)) & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{I}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3N imes15} & & & & &|& mathbf{0}_{3N imes3N} end{bmatrix} egin{bmatrix} deltamathbf{ heta}(t) \ ilde{mathbf{b}}_g(t) \ {}^G ilde{mathbf{v}}_I(t) \ ilde{mathbf{b}}_a(t) \ {}^G ilde{mathbf{p}}_I(t) \ {}^G ilde{mathbf{f}}_1 \ vdots \ {}^G ilde{mathbf{f}}_N end{bmatrix} otag \ &phantom{=} + egin{bmatrix} -mathbf{I}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} \ mathbf{0}_{3 imes3} & -mathbf{I}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{C}^T({}^Ghat{mathbf{q}}_I(t)) & mathbf{0}_{3 imes3} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{I}_{3 imes3} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} \ mathbf{0}_{3N imes12} end{bmatrix} egin{bmatrix} mathbf{n}_g \ mathbf{n}_{wg} \ mathbf{n}_a \ mathbf{n}_{wa} end{bmatrix} otag \ &= mathbf{F} egin{bmatrix} deltamathbf{ heta}(t) \ ilde{mathbf{b}}_g(t) \ {}^G ilde{mathbf{v}}_I(t) \ ilde{mathbf{b}}_a(t) \ {}^G ilde{mathbf{p}}_I(t) \ {}^G ilde{mathbf{f}}_1 \ vdots \ {}^G ilde{mathbf{f}}_N end{bmatrix} + mathbf{G} egin{bmatrix} mathbf{n}_g \ mathbf{n}_{wg} \ mathbf{n}_a \ mathbf{n}_{wa} end{bmatrix} end{align}]

[egin{align} mathbf{F}_s &= egin{bmatrix} -hat{mathbf{omega}}(t)^{wedge} & mathbf{I}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} \ [mathbf{C}^Tleft({}^Ghat{mathbf{q}}_I(t) ight)hat{mathbf{a}}(t)]^{wedge} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & -mathbf{C}^T({}^Ghat{mathbf{q}}_I(t)) & mathbf{0}_{3 imes3} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{I}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} end{bmatrix} \ mathbf{G}_s &= egin{bmatrix} -mathbf{I}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} \ mathbf{0}_{3 imes3} & -mathbf{I}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{C}^T({}^Ghat{mathbf{q}}_I(t)) & mathbf{0}_{3 imes3} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{I}_{3 imes3} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} end{bmatrix} end{align}]

从时刻 (t) 积分到时刻 (t + Delta t)。

[egin{align} dot{ ilde{mathbf{x}}}(t) &= mathbf{F}(t) ilde{mathbf{x}}(t) + mathbf{G} mathbf{n} \ ddot{ ilde{mathbf{x}}}(t) &= mathbf{F}(t) dot{ ilde{mathbf{x}}}(t) + dot{mathbf{F}}(t) ilde{mathbf{x}}(t) otag \ &= mathbf{F}(t) (mathbf{F}(t) ilde{mathbf{x}}(t) + mathbf{G} mathbf{n}) + dot{mathbf{F}}(t) ilde{mathbf{x}}(t) otag \ &= mathbf{F}(t)^2 ilde{mathbf{x}}(t) + mathbf{F}(t) mathbf{G} mathbf{n} + dot{mathbf{F}}(t) ilde{mathbf{x}}(t) \ ilde{mathbf{x}}(t+Delta t) &= ilde{mathbf{x}}(t) + dot{ ilde{mathbf{x}}}(t) Delta t + {1 over 2!}ddot{ ilde{mathbf{x}}}(t) Delta t^2 + {1 over 3!} ilde{mathbf{x}}(t)^{cdotcdotcdot} Delta t^3 + cdots otag \ &= ilde{mathbf{x}}(t) + mathbf{F}(t) ilde{mathbf{x}}(t) Delta t + {1 over 2!}mathbf{F}(t)^2 ilde{mathbf{x}}(t) Delta t^2 + {1 over 3!}mathbf{F}(t)^3 ilde{mathbf{x}}(t) Delta t^3 + cdots + Omicron(mathbf{n}) otag \ &= left[ mathbf{I} + mathbf{F}(t)Delta t + {1 over 2!}mathbf{F}(t)^2Delta t^2 + {1 over 3!}mathbf{F}(t)^3Delta t^3 + dots ight] ilde{mathbf{x}}(t) + Omicron(mathbf{n}) otag \ &simeq mathbf{Phi}(t) ilde{mathbf{x}}(t) end{align}]

目标是求 State Transition Matrix (mathbf{Phi}(t)) 。

令 (mathbf{A} = -hat{mathbf{omega}}(t)^{wedge}, mathbf{B} = [mathbf{C}^Tleft({}^Ghat{mathbf{q}}_I(t) ight)hat{mathbf{a}}(t)]^{wedge}, mathbf{C} = -mathbf{C}^T({}^Ghat{mathbf{q}}_I(t)))，将 (mathbf{F}(t)) 简写，并计算 (mathbf{F}(t)) 的各个次方。

[egin{align} mathbf{F} &= egin{bmatrix} mathbf{A} & mathbf{I}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{B} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{C} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{I}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3N imes15} & & & & &|& mathbf{0}_{3N imes3N} end{bmatrix} \ mathbf{F}^2 &= egin{bmatrix} mathbf{A}^2 & mathbf{A} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{B}mathbf{A} & mathbf{B} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{B} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{C} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3N imes15} & & & & &|& mathbf{0}_{3N imes3N} end{bmatrix} \ mathbf{F}^3 &= egin{bmatrix} mathbf{A}^3 & mathbf{A}^2 & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{B}mathbf{A}^2 & mathbf{B}mathbf{A} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{B}mathbf{A} & mathbf{B} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3N imes15} & & & & &|& mathbf{0}_{3N imes3N} end{bmatrix} \ mathbf{F}^4 &= egin{bmatrix} mathbf{A}^4 & mathbf{A}^3 & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{B}mathbf{A}^3 & mathbf{B}mathbf{A}^2 & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{B}mathbf{A}^2 & mathbf{B}mathbf{A} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3N imes15} & & & & &|& mathbf{0}_{3N imes3N} end{bmatrix} end{align}]

可以总结出规律。

[egin{align} mathbf{F} &= egin{bmatrix} mathbf{A} & mathbf{I}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{B} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{C} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{I}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3N imes15} & & & & &|& mathbf{0}_{3N imes3N} end{bmatrix} \ mathbf{F}^2 &= egin{bmatrix} mathbf{A}^2 & mathbf{A} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{B}mathbf{A} & mathbf{B} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{B} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{C} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3N imes15} & & & & &|& mathbf{0}_{3N imes3N} end{bmatrix} \ mathbf{F}^k &= egin{bmatrix} mathbf{A}^k & mathbf{A}^{k-1} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{B}mathbf{A}^{k-1} & mathbf{B}mathbf{A}^{k-2} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{B}mathbf{A}^{k-2} & mathbf{B}mathbf{A}^{k-3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3N imes15} & & & & &|& mathbf{0}_{3N imes3N} end{bmatrix}, k ge 3 end{align}]

于是可以计算 (mathbf{Phi}(t))。

[egin{align} mathbf{Phi}(t) &= mathbf{I} + mathbf{F}Delta t + {1 over 2!}mathbf{F}^2Delta t^2 + {1 over 3!}mathbf{F}^3Delta t^3 + dots otag \ &= egin{bmatrix} mathbf{I} + mathbf{A} Delta t + {1 over 2!}mathbf{A}^2 Delta t^2 + cdots & mathbf{I} Delta t + {1 over 2!} mathbf{A} Delta t^2 + {1 over 3!} mathbf{A}^2 Delta t^3 + cdots & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{I}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{B} Delta t + {1 over 2!}mathbf{B}mathbf{A} Delta t^2 + {1 over 3!}mathbf{B}mathbf{A}^2 Delta t^3 + cdots & {1 over 2!}mathbf{B} Delta t^2 + {1 over 3!}mathbf{B}mathbf{A} Delta t^3 + {1 over 4!}mathbf{B}mathbf{A}^2 Delta t^4 + cdots & mathbf{I}_{3 imes3} & mathbf{C} Delta t & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{I}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ {1 over 2!}mathbf{B} Delta t^2 + {1 over 3!}mathbf{B}mathbf{A} Delta t^3 + {1 over 4!}mathbf{B}mathbf{A}^2 Delta t^4 + cdots & {1 over 3!}mathbf{B} Delta t^3 + {1 over 4!}mathbf{B}mathbf{A} Delta t^4 + {1 over 5!}mathbf{B}mathbf{A}^2 Delta t^5 + cdots & mathbf{I}_{3 imes3} Delta t & mathbf{C} Delta t^2 & mathbf{I}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3N imes15} & & & & &|& mathbf{0}_{3N imes3N} end{bmatrix} otag \ &= egin{bmatrix} mathbf{Phi}(t)^{(1,1)} & mathbf{Phi}(t)^{(1,2)} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{I}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{Phi}(t)^{(3,1)} & mathbf{Phi}(t)^{(3,2)} & mathbf{I}_{3 imes3} & mathbf{Phi}(t)^{(3,4)} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} & mathbf{I}_{3 imes3} & mathbf{0}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{Phi}(t)^{(5,1)} & mathbf{Phi}(t)^{(5,2)} & mathbf{I}_{3 imes3} Delta t & mathbf{Phi}(t)^{(5,4)} & mathbf{I}_{3 imes3} &|& mathbf{0}_{3 imes 3N} \ mathbf{0}_{3N imes15} & & & & &|& mathbf{0}_{3N imes3N} end{bmatrix} end{align}]

参考论文 [1] 的公式 (366) 整理以上矩阵块。

[egin{align} mathbf{Phi}(t)^{(1,1)} &= mathbf{I} + mathbf{A} Delta t + {1 over 2!}mathbf{A}^2 Delta t^2 + cdots otag \ &= ext{exp}(-hat{mathbf{omega}}(t)^{wedge}Delta t) \ mathbf{Phi}(t)^{(1,2)} &= mathbf{I} Delta t + {1 over 2!} mathbf{A} Delta t^2 + {1 over 3!} mathbf{A}^2 Delta t^3 + cdots otag \ &= mathbf{I} Delta t + {1 over 2!} {-mathbf{A}^3 over ||hat{mathbf{omega}}(t)||^2} Delta t^2 + {1 over 3!} {-mathbf{A}^3 over ||hat{mathbf{omega}}(t)||^2} mathbf{A} Delta t^3 + cdots otag \ &= mathbf{I} Delta t + {-mathbf{A} over ||hat{mathbf{omega}}(t)||^2} left({1 over 2!} mathbf{A}^2 Delta t^2 + {1 over 3!} mathbf{A}^3 Delta t^3 + cdots ight) otag \ &= mathbf{I} Delta t + {-mathbf{A} over ||hat{mathbf{omega}}(t)||^2} ( ext{exp}(mathbf{A}Delta t) - mathbf{I} - mathbf{A}Delta t) otag \ &= mathbf{I} Delta t + {hat{mathbf{omega}}(t)^{wedge} over ||hat{mathbf{omega}}(t)||^2} ( ext{exp}(-hat{mathbf{omega}}(t)^{wedge}Delta t) - mathbf{I} + hat{mathbf{omega}}(t)^{wedge}Delta t) \ mathbf{Phi}(t)^{(3,1)} &= mathbf{B} Delta t + {1 over 2!}mathbf{B}mathbf{A} Delta t^2 + {1 over 3!}mathbf{B}mathbf{A}^2 Delta t^3 + cdots otag \ &= mathbf{B} left(mathbf{I}Delta t + {1 over 2!}mathbf{A} Delta t^2 + {1 over 3!}mathbf{A}^2 Delta t^3 + cdots ight) otag \ &= mathbf{B} mathbf{Phi}(t)^{(1,2)} otag \ &= mathbf{B}left(mathbf{I} Delta t + {hat{mathbf{omega}}(t)^{wedge} over ||hat{mathbf{omega}}(t)||^2} ( ext{exp}(-hat{mathbf{omega}}(t)^{wedge}Delta t) - mathbf{I} + hat{mathbf{omega}}(t)^{wedge}Delta t) ight) otag \ &= [mathbf{C}^Tleft({}^Ghat{mathbf{q}}_I(t) ight)hat{mathbf{a}}(t)]^{wedge} left(mathbf{I} Delta t + {hat{mathbf{omega}}(t)^{wedge} over ||hat{mathbf{omega}}(t)||^2} ( ext{exp}(-hat{mathbf{omega}}(t)^{wedge}Delta t) - mathbf{I} + hat{mathbf{omega}}(t)^{wedge}Delta t) ight) \ mathbf{Phi}(t)^{(3,2)} &= mathbf{Phi}(t)^{(5,1)} = {1 over 2!}mathbf{B} Delta t^2 + {1 over 3!}mathbf{B}mathbf{A} Delta t^3 + {1 over 4!}mathbf{B}mathbf{A}^2 Delta t^4 + cdots otag \ &= mathbf{B} left( {1 over 2!}mathbf{I} Delta t^2 + {1 over 3!}mathbf{A} Delta t^3 + {1 over 4!}mathbf{A}^2 Delta t^4 + cdots ight) otag \ &= mathbf{B} left( {1 over 2!}mathbf{I} Delta t^2 + {1 over 3!}{-mathbf{A}^3 over ||hat{mathbf{omega}}(t)||^2} Delta t^3 + {1 over 4!}{-mathbf{A}^3 over ||hat{mathbf{omega}}(t)||^2}mathbf{A} Delta t^4 + cdots ight) otag \ &= mathbf{B} left[ {1 over 2!}mathbf{I} Delta t^2 + {-1 over ||hat{mathbf{omega}}(t)||^2} left({1 over 3!}mathbf{A}^3 Delta t^3 + {1 over 4!}mathbf{A}^4 Delta t^4 + cdots ight) ight] otag \ &= mathbf{B} left[ {1 over 2!}mathbf{I} Delta t^2 + {-1 over ||hat{mathbf{omega}}(t)||^2} left( ext{exp}(mathbf{A}Delta t) - mathbf{I} - mathbf{A}Delta t - {1 over 2!}mathbf{A}^2Delta t^2 ight) ight] otag \ &= mathbf{B} left[ {1 over 2!}mathbf{I} Delta t^2 + {-1 over ||hat{mathbf{omega}}(t)||^2} left( ext{exp}(-hat{mathbf{omega}}(t)^{wedge}Delta t) - mathbf{I} + hat{mathbf{omega}}(t)^{wedge}Delta t - {1 over 2!}(hat{mathbf{omega}}(t)^{wedge})^2Delta t^2 ight) ight] otag \ &= [mathbf{C}^Tleft({}^Ghat{mathbf{q}}_I(t) ight)hat{mathbf{a}}(t)]^{wedge} left[ {1 over 2!}mathbf{I} Delta t^2 + {-1 over ||hat{mathbf{omega}}(t)||^2} left( ext{exp}(-hat{mathbf{omega}}(t)^{wedge}Delta t) - mathbf{I} + hat{mathbf{omega}}(t)^{wedge}Delta t - {1 over 2!}(hat{mathbf{omega}}(t)^{wedge})^2Delta t^2 ight) ight] \ mathbf{Phi}(t)^{(3,4)} &= mathbf{C} Delta t otag \ &= -mathbf{C}^T({}^Ghat{mathbf{q}}_I(t)) Delta t \ mathbf{Phi}(t)^{(5,4)} &= mathbf{C} Delta t^2 otag \ &= -mathbf{C}^T({}^Ghat{mathbf{q}}_I(t)) Delta t^2 end{align}]

[egin{align} mathbf{Phi}(t)^{(5,2)} &= {1 over 3!}mathbf{B} Delta t^3 + {1 over 4!}mathbf{B}mathbf{A} Delta t^4 + {1 over 5!}mathbf{B}mathbf{A}^2 Delta t^5 + cdots otag \ &= mathbf{B} left( {1 over 3!}mathbf{I}Delta t^3 + {1 over 4!}mathbf{A} Delta t^4 + {1 over 5!}mathbf{A}^2 Delta t^5 + cdots ight) otag \ &= mathbf{B} left( {1 over 3!}mathbf{I}Delta t^3 + {1 over 4!}{-mathbf{A}^3 over ||hat{mathbf{omega}}(t)||^2} Delta t^4 + {1 over 5!}{-mathbf{A}^3 over ||hat{mathbf{omega}}(t)||^2} mathbf{A} Delta t^5 + cdots ight) otag \ &= mathbf{B} left( {1 over 3!}mathbf{I}Delta t^3 + {1 over 4!} mathbf{A} {-mathbf{A}^2 over ||hat{mathbf{omega}}(t)||^2} Delta t^4 + {1 over 5!} mathbf{A} {-mathbf{A}^2 over ||hat{mathbf{omega}}(t)||^2} mathbf{A} Delta t^5 + cdots ight) otag \ &= mathbf{B} left( {1 over 3!}mathbf{I}Delta t^3 + {1 over 4!} {-mathbf{A}^3 over ||hat{mathbf{omega}}(t)||^2} {-mathbf{A}^2 over ||hat{mathbf{omega}}(t)||^2} Delta t^4 + {1 over 5!} {-mathbf{A}^3 over ||hat{mathbf{omega}}(t)||^2} {-mathbf{A}^2 over ||hat{mathbf{omega}}(t)||^2} mathbf{A} Delta t^5 + cdots ight) otag \ &= mathbf{B} left[ {1 over 3!}mathbf{I}Delta t^3 + {mathbf{A} over ||hat{mathbf{omega}}(t)||^4} left( {1 over 4!} mathbf{A}^4 Delta t^4 + {1 over 5!} mathbf{A}^5 Delta t^5 + cdots ight) ight] otag \ &= mathbf{B} left[ {1 over 3!}mathbf{I}Delta t^3 + {mathbf{A} over ||hat{mathbf{omega}}(t)||^4} left( ext{exp}(mathbf{A}Delta t) - mathbf{I} - mathbf{A} Delta t - {1 over 2!} mathbf{A}^2 Delta t^2 - {1 over 3!} mathbf{A}^3 Delta t^3 ight) ight] otag \ &= mathbf{B} left[ {1 over 3!}mathbf{I}Delta t^3 + {-hat{mathbf{omega}}(t)^{wedge} over ||hat{mathbf{omega}}(t)||^4} left( ext{exp}(-hat{mathbf{omega}}(t)^{wedge}Delta t) - mathbf{I} + hat{mathbf{omega}}(t)^{wedge} Delta t - {1 over 2!} (hat{mathbf{omega}}(t)^{wedge})^2 Delta t^2 + {1 over 3!} (hat{mathbf{omega}}(t)^{wedge})^3 Delta t^3 ight) ight] otag \ &= [mathbf{C}^Tleft({}^Ghat{mathbf{q}}_I(t) ight)hat{mathbf{a}}(t)]^{wedge} left[ {1 over 3!}mathbf{I}Delta t^3 + {-hat{mathbf{omega}}(t)^{wedge} over ||hat{mathbf{omega}}(t)||^4} left( ext{exp}(-hat{mathbf{omega}}(t)^{wedge}Delta t) - mathbf{I} + hat{mathbf{omega}}(t)^{wedge} Delta t - {1 over 2!} (hat{mathbf{omega}}(t)^{wedge})^2 Delta t^2 - {1 over 3!} ||hat{mathbf{omega}}(t)||^2 hat{mathbf{omega}}(t)^{wedge} Delta t^3 ight) ight] \ end{align}]

参考文献

[1] Sola, Joan. "Quaternion kinematics for the error-state Kalman filter." arXiv preprint arXiv:1711.02508 (2017).

[2] Madyastha, Venkatesh, et al. "Extended Kalman filter vs. error state Kalman filter for aircraft attitude estimation." AIAA Guidance, Navigation, and Control Conference. 2011.