MSCKF原理解析(参照论文)

原理解析(参照论文)"/>

MSCKF原理解析(参照论文)

参考论文地址 .pdf

算法特点

• 基于EKF框架（个人觉得叫ESKF更妥，文章中进行更新的状态都是误差状态）
• 提出了一种不需要将3D特征点包含到状态向量中的测量模型，使得计算规模小，复杂度低．

算法组成

• 状态的定义
• IMU预测(包括状态预测、协方差的更新)
• 状态增广
• 测量模型的构建
• EKF更新

下面将详细描述上述每个过程

状态定义

• IMU状态:
  X I M U = [ G I ⁣ q ˉ T b g T G ⁣ v I T b a T G ⁣ p I T ] T X_{IMU}=\begin{bmatrix} {}_G^I\!\bar{q}^{T} & \mathbf{b}_g^{T} & {}^{G}\!\mathbf{v}_{I}^{T} & \mathbf{b}_{a}^{T} & {}^{G}\!\mathbf{p}_{I}^{T} \end{bmatrix}^{T} XIMU​=[GI​qˉ​T​bgT​​GvIT​​baT​​GpIT​​]T
  式中 p , v , q ˉ \mathbf{p, v, \bar{q}} p,v,qˉ​分别为IMU相对于世界坐标系的位置、速度和归一化四元素姿态， b g \mathbf{b}_{g} bg​, b a \mathbf{b}_{a} ba​分别为IMU陀螺仪和加速度计偏置．
• IMU误差状态:
  X ~ I M U = [ δ θ I T b g T ~ G ⁣ v I T ~ b a T ~ G ⁣ p I T ~ ] T \widetilde{X}_{IMU}=\begin{bmatrix} \delta\boldsymbol{\theta}_{I}^{T} & \widetilde{\mathbf{b}_g^{T}} & \widetilde{{}^{G}\!\mathbf{v}_{I}^{T}} & \widetilde{\mathbf{b}_{a}^{T}} & \widetilde{{}^{G}\!\mathbf{p}_{I}^{T}} \end{bmatrix}^{T} X IMU​=[δθIT​​bgT​ ​​GvIT​ ​​baT​ ​​GpIT​ ​​]T
  对于位置、速度和偏置状态，其误差状态直接通过标准加性误差 x ~ = x − x ^ \widetilde{x}=x-\hat{x} x =x−x^定义．而对于姿态状态，其满足的关系是 q ˉ = δ q ⊗ q ˉ ^ \bar{q}=\delta{q}\otimes\hat{\bar{q}} qˉ​=δq⊗qˉ​^​，其中 δ q ≃ [ 1 2 δ θ 1 ] T \delta{q}\simeq\begin{bmatrix} \frac{1}{2}\delta\boldsymbol{\theta} & 1 \end{bmatrix}^{T} δq≃[21​δθ​1​]T．因此这里的姿态误差状态可以用 δ q \delta{q} δq或者 δ θ \delta{\boldsymbol{\theta}} δθ来表示，但 δ q \delta{q} δq是超参数表示，不利于后期的优化过程，所以使用 δ θ \delta{\boldsymbol{\theta}} δθ这种最小表达形式．
• EKF状态向量:
  X k ^ = [ X ^ I M U K T G C 1 q ˉ ^ T G p ^ C 1 T . . . G C N q ˉ ^ T G p ^ C N T ] \hat{\mathbf{X}_{k}}=\begin{bmatrix} \hat{\mathbf{X}}_{IMU_{K}}^{T} & {}_{G}^{C_{1}}\hat{\bar{q}}^{T} & {}^{G}\hat{\mathbf{p}}_{C_{1}}^{T} & ... & {}_{G}^{C_{N}}\hat{\bar{q}}^{T} & {}^{G}\hat{\mathbf{p}}_{C_{N}}^{T} \end{bmatrix} Xk​^​=[X^IMUK​T​​GC1​​qˉ​^​T​Gp^​C1​T​​...​GCN​​qˉ​^​T​Gp^​CN​T​​]
  该算法框架中长期维护的状态向量包括k时刻(当前时刻)IMU状态 X ^ I M U K \hat{\mathbf{X}}_{IMU_{K}} X^IMUK​​和N个相机状态 [ G C q ˉ ^ T G p ^ C T ] \begin{bmatrix} {}_{G}^{C}\hat{\bar{q}}^{T} & {}^{G}\hat{\mathbf{p}}_{C}^{T}\end{bmatrix} [GC​qˉ​^​T​Gp^​CT​​]
• EKF误差状态向量:
  X k ~ = [ X ~ I M U K T δ θ C 1 T G p ~ C 1 T . . . δ θ C N T G p ~ C N T ] \widetilde{\mathbf{X}_{k}}=\begin{bmatrix} \widetilde{\mathbf{X}}_{IMU_{K}}^{T} & \delta{\boldsymbol{\theta}}_{C_{1}}^{T} & {}^{G}\widetilde{\mathbf{p}}_{C_{1}}^{T} & ... & \delta{\boldsymbol{\theta}}_{C_{N}}^{T} & {}^{G}\widetilde{\mathbf{p}}_{C_{N}}^{T} \end{bmatrix} Xk​ ​=[X IMUK​T​​δθC1​T​​Gp ​C1​T​​...​δθCN​T​​Gp ​CN​T​​]
  该算法框架中利用的误差状态向量包括k时刻IMU误差状态 X ~ I M U K \widetilde{\mathbf{X}}_{IMU_{K}} X IMUK​​和N个相机误差状态 [ δ θ C T G p ~ C T ] \begin{bmatrix} \delta\boldsymbol{\theta}_{C}^{T}& {}^{G}\widetilde{\mathbf{p}}_{C}^{T}\end{bmatrix} [δθCT​​Gp ​CT​​]

IMU预测(当接收到IMU信息时)

1. 状态预测

• 真实状态系统模型
  G I q ˉ ˙ ( t ) = 1 2 Ω ( ω ( t ) ) G I q ˉ ( t ) b g ˙ ( t ) = n w g ( t ) G v I ˙ ( t ) = G a ( t ) b a ˙ ( t ) = n w a ( t ) G p I ˙ ( t ) = G v I T \begin{matrix} {}_{G}^{I}\dot{\bar{q}}(t)=\frac{1}{2}\Omega(\boldsymbol{\omega}(t)){}_{G}^{I}\bar{q}(t) \\ \dot{\boldsymbol{b}_{g}}(t)=\mathbf{n}_{wg}(t) \\ {^{G}}\dot{\mathbf{v}_{I}}(t)={}^{G}\mathbf{a}(t) \\ \dot{\mathbf{b}_{a}}(t)=\mathbf{n}_{wa}(t) \\ {}^{G}\dot{\mathbf{p}_{I}}(t)={}^{G}\mathbf{v}_{I}^{T} \end{matrix} GI​qˉ​˙​(t)=21​Ω(ω(t))GI​qˉ​(t)bg​˙​(t)=nwg​(t)GvI​˙​(t)=Ga(t)ba​˙​(t)=nwa​(t)GpI​˙​(t)=GvIT​​
  式中 ω ( t ) = [ ω x ω y ω z ] \boldsymbol{\omega}(t)=\begin{bmatrix} \omega_{x} & \omega_{y} & \omega_{z} \end{bmatrix} ω(t)=[ωx​​ωy​​ωz​​]为实际角速度值， G a ( t ) {}^{G}\mathbf{a}(t) Ga(t)为实际加速度值， Ω ( ω ) = [ − ⌊ ω × ⌋ ω − ω T 0 ] \Omega(\boldsymbol{\omega})=\begin{bmatrix} -\left \lfloor \boldsymbol{\omega}\times \right \rfloor & \boldsymbol{\omega} \\ -\boldsymbol{\omega}^{T} & 0 \end{bmatrix} Ω(ω)=[−⌊ω×⌋−ωT​ω0​]， ⌊ ω × ⌋ = [ 0 − ω z ω y ω z 0 − ω x − ω y ω x 0 ] \left \lfloor \boldsymbol{\omega}\times \right \rfloor = \begin{bmatrix} 0 & -\omega_{z} & \omega_{y} \\ \omega_{z} & 0 & -\omega_{x} \\ -\omega_{y} & \omega_{x} & 0\\ \end{bmatrix} ⌊ω×⌋=⎣⎡​0ωz​−ωy​​−ωz​0ωx​​ωy​−ωx​0​⎦⎤​
• 名义状态系统模型
  G I q ˉ ^ ˙ = 1 2 Ω ( ω ^ ) G I q ˉ ^ b g ^ ˙ = 0 3 × 1 G v I ^ ˙ = C q ^ T a ^ + G g b ^ ˙ a = 0 3 × 1 G p ^ ˙ I = G v ^ I \begin{matrix} {}_{G}^{I}\dot{\hat{\bar{q}}}=\frac{1}{2}\Omega(\hat{\boldsymbol{\omega}}){}_{G}^{I}\hat{\bar{q}} \\ \dot{\hat{\boldsymbol{b}_{g}}}=\mathbf{0}_{3\times 1} \\ {}^{G}\dot{\hat{\mathbf{v}_{I}}}=\mathbf{C}_{\hat{q}}^{T}\hat{\mathbf{a}}+{}^{G}\mathbf{g} \\ \dot{\hat{\mathbf{b}}}_{a}=\mathbf{0}_{3\times 1}\\ {}^{G}\dot{\hat{\mathbf{p}}}_{I}={}^{G}\hat{\mathbf{v}}_{I} \end{matrix} GI​qˉ​^​˙​=21​Ω(ω^)GI​qˉ​^​bg​^​˙​=03×1​GvI​^​˙​=Cq^​T​a^+Ggb^˙a​=03×1​Gp^​˙​I​=Gv^I​​
  相比与论文中的模型，该模型忽略了地球自转影响，对该算法影响不大．式中 C q ^ = C ( G I q ˉ ^ ) \mathbf{C}_{\hat{q}}=\mathbf{C}({}_{G}^{I}\hat{\bar{q}}) Cq^​​=C(GI​qˉ​^​)为通过姿态估计值转换成的旋转矩阵，式中 a ^ = a m − b ^ a \hat{\mathbf{a}}=\mathbf{a}_{m}-\hat{\mathbf{b}}_{a} a^=am​−b^a​， ω ^ = ω m − b ^ g − C q ^ ω G \hat{\boldsymbol{\omega}}=\boldsymbol{\omega}_{m}-\hat{\mathbf{b}}_{g}-\mathbf{C}_{\hat{q}}\boldsymbol{\omega}_{G} ω^=ωm​−b^g​−Cq^​​ωG​， ( a m , ω m ) (\mathbf{a}_{m},\boldsymbol{\omega}_{m}) (am​,ωm​)分别为加速度计和陀螺仪测量值．
• 误差状态系统模型(即EKF的预测模型)
  根据误差状态＝真实状态－名义状态定义，可得
  X ~ ˙ I M U = F X ~ I M U + G n I M U \dot{\widetilde{\mathbf{X}}}_{IMU}=\mathbf{F}\widetilde{\mathbf{X}}_{IMU}+\mathbf{G}\mathbf{n}_{IMU} X ˙IMU​=FX IMU​+GnIMU​
  其中:
  F = [ − ⌊ ω ^ × ⌋ − I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 − C q ^ T ⌊ a ^ × ⌋ 0 3 × 3 0 3 × 3 − C q ^ T 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 0 3 × 3 0 3 × 3 ] , G = [ − I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 − C q ^ T 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 ] \mathbf{F}=\begin{bmatrix} -\left \lfloor \hat{\boldsymbol{\omega}}\times \right \rfloor & -\mathbf{I}_{3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} \\ \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} \\ -\mathbf{C}_{\hat{q}}^{T}\left \lfloor \hat{\mathbf{a}}\times \right \rfloor & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & -\mathbf{C}_{\hat{q}}^{T} & \mathbf{0}_{3\times 3} \\ \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} \\ \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{I}_{3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} \end{bmatrix},\mathbf{G}=\begin{bmatrix} -\mathbf{I}_{3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} \\ \mathbf{0}_{3\times 3} & \mathbf{I}_{3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} \\ \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & -\mathbf{C}_{\hat{q}}^{T} & \mathbf{0}_{3\times 3} \\ \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{I}_{3} \\ \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} \end{bmatrix} F=⎣⎢⎢⎢⎢⎡​−⌊ω^×⌋03×3​−Cq^​T​⌊a^×⌋03×3​03×3​​−I3​03×3​03×3​03×3​03×3​​03×3​03×3​03×3​03×3​I3​​03×3​03×3​−Cq^​T​03×3​03×3​​03×3​03×3​03×3​03×3​03×3​​⎦⎥⎥⎥⎥⎤​,G=⎣⎢⎢⎢⎢⎡​−I3​03×3​03×3​03×3​03×3​​03×3​I3​03×3​03×3​03×3​​03×3​03×3​−Cq^​T​03×3​03×3​​03×3​03×3​03×3​I3​03×3​​⎦⎥⎥⎥⎥⎤​
  n I M U = [ n g T n w g T n a T n w a T ] 为 I M U 误 差 模 型 参 数 \mathbf{n}_{IMU}=\begin{bmatrix} \mathbf{n}_{g}^{T} & \mathbf{n}_{wg}^{T} & \mathbf{n}_{a}^{T} & \mathbf{n}_{wa}^{T} \end{bmatrix}为IMU误差模型参数 nIMU​=[ngT​​nwgT​​naT​​nwaT​​]为IMU误差模型参数
  详细推导可查看我博客Quaternion kinematics for ESKF－预测中"连续时间内的系统运动方程"部分内容．

2. 协方差预测

• k时刻协方差
  P k ∣ k = [ P I I k ∣ k P I C k ∣ k P I C k ∣ k T P C C k ∣ k ] \mathbf{P}_{k|k}=\begin{bmatrix} \mathbf{P}_{II_{k|k}} & \mathbf{P}_{IC_{k|k}} \\ \mathbf{P}^{T}_{IC_{k|k}} & \mathbf{P}_{CC_{k|k}} \end{bmatrix} Pk∣k​=[PIIk∣k​​PICk∣k​T​​PICk∣k​​PCCk∣k​​​]
• 预测的k+1时刻协方差
  P k + 1 ∣ k = [ P I I k + 1 ∣ k Φ ( t k + T , t k ) P I C k ∣ k P I C k ∣ k T Φ ( t k + T , t k ) T P C C k ∣ k ] \mathbf{P}_{k+1|k}=\begin{bmatrix} \mathbf{P}_{II_{k+1|k}} & \boldsymbol{\Phi}(t_{k}+T,t_{k})\mathbf{P}_{IC_{k|k}}\\ \mathbf{P}^{T}_{IC_{k|k}}\boldsymbol{\Phi}(t_{k}+T,t_{k})^{T} & \mathbf{P}_{CC_{k|k}} \end{bmatrix} Pk+1∣k​=[PIIk+1∣k​​PICk∣k​T​Φ(tk​+T,tk​)T​Φ(tk​+T,tk​)PICk∣k​​PCCk∣k​​​]
  其中 P I I k + 1 ∣ k \mathbf{P}_{II_{k+1|k}} PIIk+1∣k​​通过李雅普诺夫方程计算:
  P ˙ I I = F P I I + P I I F T + G Q I M U G T \dot{\mathbf{P}}_{II}=\mathbf{FP}_{II}+\mathbf{P}_{II}\mathbf{F}^{T}+\mathbf{GQ}_{IMU}\mathbf{G}^{T} P˙II​=FPII​+PII​FT+GQIMU​GT
  Φ ( t k + T , t k ) \boldsymbol{\Phi}(t_{k}+T,t_{k}) Φ(tk​+T,tk​)通过下面方程进行数值积分可得(初始值 Φ ( t k , t k ) = I 15 × 15 \boldsymbol{\Phi}(t_{k}, t_{k})=\mathbf{I}_{15\times 15} Φ(tk​,tk​)=I15×15​):
  Φ ˙ ( t k + τ , t k ) = F Φ ( t k + τ , t k ) , τ ∈ [ 0 , T ] \dot{\boldsymbol{\Phi}}(t_{k}+\tau, t_{k})=\mathbf{F}\boldsymbol{\Phi}(t_{k}+\tau, t_{k}),\tau\in[0,T] Φ˙(tk​+τ,tk​)=FΦ(tk​+τ,tk​),τ∈[0,T]

状态增广(当接收到新图片信息时)

• 状态增广
  当接收到新图片时，需要将新的相机位姿添加到滤波的状态向量中，添加的状态按如下方法获取:
  G C q ˉ ^ = I C q ˉ ⊗ G I q ˉ ^ , G p ^ C = G p ^ I + C q ^ T I p C {}_{G}^{C}\hat{\bar{q}}={}_{I}^{C}\bar{q}\otimes{}_{G}^{I}\hat{\bar{q}},{}^{G}\hat{\mathbf{p}}_{C}={}^{G}\hat{\mathbf{p}}_{I}+\mathbf{C}_{\hat{q}}^{T}{}^{I}\mathbf{p}_{C} GC​qˉ​^​=IC​qˉ​⊗GI​qˉ​^​,Gp^​C​=Gp^​I​+Cq^​T​IpC​
• 协方差增广
  由于状态的增广，使得协方差矩阵应对应增广，方法如下:
  P k ∣ k = [ I 6 N + 15 J ] P k ∣ k [ I 6 N + 15 J ] T \mathbf{P}_{k|k}=\begin{bmatrix}\mathbf{I}_{6N+15} \\ \mathbf{J}\end{bmatrix} \mathbf{P}_{k|k} \begin{bmatrix}\mathbf{I}_{6N+15} \\ \mathbf{J}\end{bmatrix}^{T} Pk∣k​=[I6N+15​J​]Pk∣k​[I6N+15​J​]T
  其中:
  J = [ C ( I C q ˉ ) 0 3 × 9 0 3 × 3 0 3 × 6 N ⌊ C q ^ T I p C × ⌋ 0 3 × 9 I 3 0 3 × 6 N ] \mathbf{J}=\begin{bmatrix} \mathbf{C}({}_{I}^{C}\bar{\mathbf{q}}) & \mathbf{0}_{3\times 9} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 6N} \\ \left \lfloor \mathbf{C}_{\hat{q}}^{T} {}^{I}\mathbf{p}_{C}\times \right \rfloor & \mathbf{0}_{3\times 9} & \mathbf{I}_{3} & \mathbf{0}_{3\times 6N} \end{bmatrix} J=[C(IC​qˉ​)⌊Cq^​T​IpC​×⌋​03×9​03×9​​03×3​I3​​03×6N​03×6N​​]

测量模型的构建

这部分是该算法的核心，保证了算法的高效运行．传统的基于EKF框架的vio算法都会将特征点作为状态向量的一部分在整个算法运行过程中进行更新矫正，这就导致了状态向量的维度急速提高，要知道在进行滤波算法的过程中，是需要计算观测方程的jacobi矩阵的，维度一旦太大，必然导致计算复杂性快速提高，导致算法的效率降低，不利于大规模场景的使用．
该算法不维护特征点信息，而是当一个特征点未被继续观察到时，通过观察到该特征点的相机状态利用最小二乘方法获取特征点的位置，然后利用该位置信息和观测到的特征信息来构建观测模型．

• 实际测量模型
  假设单个特征 f j f_{j} fj​位置 p f j \mathbf{p}_{f_{j}} pfj​​被 S j S_{j} Sj​个相机状态 ( G C i q ˉ , G p C i ) , i ∈ S j ({}_{G}^{C_{i}}\bar{q},{}^{G}\mathbf{p}_{C_{i}}),i\in S_{j} (GCi​​qˉ​,GpCi​​),i∈Sj​观测到，则可建立如下实际测量模型:
  z i ( j ) = 1 C i Z j [ C i X j C i Y j ] + n i ( j ) , i ∈ S j \mathbf{z}^{(j)}_{i}=\frac{1}{{}^{C_{i}}Z_{j}}\begin{bmatrix} {}^{C_{i}}X_{j} \\ {}^{C_{i}}Y_{j} \end{bmatrix}+\mathbf{n}^{(j)}_{i},i\in S_{j} zi(j)​=Ci​Zj​1​[Ci​Xj​Ci​Yj​​]+ni(j)​,i∈Sj​
  其中 [ C i X j , C i Y j , C i Z j ] [ {}^{C_{i}}X_{j}, {}^{C_{i}}Y_{j}, {}^{C_{i}}Z_{j}] [Ci​Xj​,Ci​Yj​,Ci​Zj​]为特征点 f j f_{j} fj​在相机状态 C j C_{j} Cj​坐标系下的坐标，可通过下式计算:
  C i p f j = [ C i X j , C i Y j , C i Z j ] T = C ( G C j q ˉ ) ( G p f j − G p c i ) {}^{C_{i}}\mathbf{p}_{f_{j}}=[ {}^{C_{i}}X_{j}, {}^{C_{i}}Y_{j}, {}^{C_{i}}Z_{j}]^{T}=\mathbf{C}({}_{G}^{C_{j}}\bar{q})({}^{G}\mathbf{p}_{f_{j}}-{}^{G}\mathbf{p}_{c_{i}}) Ci​pfj​​=[Ci​Xj​,Ci​Yj​,Ci​Zj​]T=C(GCj​​qˉ​)(Gpfj​​−Gpci​​)
  另外 n i ( j ) \mathbf{n}_{i}^{(j)} ni(j)​是图像的噪声向量，满足 n i ( j ) ∼ N ( 0 , σ i m 2 I ) \mathbf{n}_{i}^{(j)}\sim N(\mathbf{0}, \sigma_{im}^{2}\mathbf{I}) ni(j)​∼N(0,σim2​I)，为高斯白噪声．
• 名义测量模型
  z ^ i ( j ) = 1 C i Z j ^ [ C i X j ^ C i Y j ^ ] , C i p ^ f j = [ C i X j ^ , C i Y j ^ , C i Z j ^ ] T = C ( G C j q ˉ ^ ) ( G p ^ f j − G p ^ c i ) \hat{\mathbf{z}}^{(j)}_{i}=\frac{1}{{}^{C_{i}}\hat{Z_{j}}}\begin{bmatrix} {}^{C_{i}}\hat{X_{j}} \\ {}^{C_{i}}\hat{Y_{j}} \end{bmatrix}, {}^{C_{i}}\hat{\mathbf{p}}_{f_{j}}=[ {}^{C_{i}}\hat{X_{j}}, {}^{C_{i}}\hat{Y_{j}}, {}^{C_{i}}\hat{Z_{j}}]^{T}=\mathbf{C}({}_{G}^{C_{j}}\hat{\bar{q}})({}^{G}\hat{\mathbf{p}}_{f_{j}}-{}^{G}\hat{\mathbf{p}}_{c_{i}}) z^i(j)​=Ci​Zj​^​1​[Ci​Xj​^​Ci​Yj​^​​],Ci​p^​fj​​=[Ci​Xj​^​,Ci​Yj​^​,Ci​Zj​^​]T=C(GCj​​qˉ​^​)(Gp^​fj​​−Gp^​ci​​)
  其中的名义状态 G C j q ˉ ^ , G p ^ f j , G p ^ c i {}_{G}^{C_{j}}\hat{\bar{q}},{}^{G}\hat{\mathbf{p}}_{f_{j}},{}^{G}\hat{\mathbf{p}}_{c_{i}} GCj​​qˉ​^​,Gp^​fj​​,Gp^​ci​​均为状态增广过程添加的相机状态．
• 观测方程
  建立观测方程如下: r i ( j ) = z i ( j ) − z ^ i ( j ) \mathbf{r}_{i}^{(j)}=\mathbf{z}_{i}^{(j)}-\hat{\mathbf{z}}_{i}^{(j)} ri(j)​=zi(j)​−z^i(j)​
  由于该方程为非线性方程，需要进行泰勒展开是线性方程，才能进行滤波，展开后方程:
  r i ( j ) ≃ H X i j X ~ + H f j j G p ~ f j + n i j \mathbf{r}_{i}^{(j)}\simeq\mathbf{H}_{\mathbf{X}_{i}}^{j}\widetilde{X}+\mathbf{H}_{f_{j}}^{j}{}^{G}\widetilde{\mathbf{p}}_{f_{j}}+\mathbf{n}_{i}^{j} ri(j)​≃HXi​j​X +Hfj​j​Gp ​fj​​+nij​

其中 H X i j , H f j j \mathbf{H}_{\mathbf{X}_{i}}^{j},\mathbf{H}_{f_{j}}^{j} HXi​j​,Hfj​j​分别为测量值 z i ( j ) \mathbf{z}_{i}^{(j)} zi(j)​相对于状态和特征位置的jacobia矩阵．
将所有观测到特征 f j f_{j} fj​的相机状态构建的观测方程累加起来如下:
r ( j ) ≃ H X j X ~ + H f j G p ~ f j + n j \mathbf{r}^{(j)}\simeq\mathbf{H}_{\mathbf{X}}^{j}\widetilde{X}+\mathbf{H}_{f}^{j}{}^{G}\widetilde{\mathbf{p}}_{f_{j}}+\mathbf{n}^{j} r(j)≃HXj​X +Hfj​Gp ​fj​​+nj
从上面的展开后的观测方程可以看出，观测过程不仅与误差状态有关还与特征位置有关，因此不能直接将上式作为最终的观测方程，否则达不到该算法的特点:不将特征加入状态向量中．为了达到算法效果，将 r ( j ) \mathbf{r}^{(j)} r(j)投影到 H f ( j ) \mathbf{H}_{f}^{(j)} Hf(j)​的左零空间中，可获得如下改进后的观测方程:
r o ( j ) = A T ( z ( j ) − z ^ ( j ) ) ≃ A T H X ( j ) X ~ + A T n ( j ) = H o ( j ) X ~ ( j ) + n o ( j ) \mathbf{r}_{o}^{(j)}=\mathbf{A}^{T}(\mathbf{z}^{(j)}-\hat{\mathbf{z}}^{(j)})\simeq\mathbf{A}^{T}\mathbf{H}_{\mathbf{X}}^{(j)}\widetilde{\mathbf{X}}+\mathbf{A}^{T}\mathbf{n}^{(j)}=\mathbf{H}_{o}^{(j)}\widetilde{X}^{(j)}+\mathbf{n}_{o}^{(j)} ro(j)​=AT(z(j)−z^(j))≃ATHX(j)​X +ATn(j)=Ho(j)​X (j)+no(j)​
其中A为半正交矩阵，它的列构成了 H f H_{f} Hf​的左零空间， r o \mathbf{r}_{o} ro​为投影后的残差项．

EKF更新

当某个特征点不被继续观测到或者相机状态太多需要消除老状态时，进行EKF的更新．由于 H o \mathbf{H}_{o} Ho​矩阵比较大，为了降低计算的复杂性，对 H o \mathbf{H}_{o} Ho​矩阵进行QR分解:
H o = [ Q 1 Q 2 ] [ T H 0 ] \mathbf{H}_{o}=\begin{bmatrix} \mathbf{Q}_{1} & \mathbf{Q}_{2} \end{bmatrix} \begin{bmatrix} \mathbf{T}_{H} \\ \mathbf{0} \end{bmatrix} Ho​=[Q1​​Q2​​][TH​0​]
其中 Q 1 , Q 2 , T H \mathbf{Q}_{1},\mathbf{Q}_{2},\mathbf{T}_{H} Q1​,Q2​,TH​分别为酉矩阵、酉矩阵和上三角矩阵．
于是原观测方程可表达为:
r o = [ Q 1 Q 2 ] [ T H 0 ] X ~ + n o ⇒ [ Q 1 T r o Q 2 T r o ] = [ T H 0 ] X ~ + [ Q 1 T n o Q 2 T n o ] \mathbf{r}_{o}=\begin{bmatrix} \mathbf{Q}_{1} & \mathbf{Q}_{2} \end{bmatrix} \begin{bmatrix} \mathbf{T}_{H} \\ \mathbf{0}\end{bmatrix}\widetilde{X}+\mathbf{n}_{o}\Rightarrow \begin{bmatrix} \mathbf{Q}_{1}^{T}\mathbf{r}_{o} \\ \mathbf{Q}_{2}^{T}\mathbf{r}_{o} \end{bmatrix}= \begin{bmatrix} \mathbf{T}_{H}\\ \mathbf{0} \end{bmatrix}\widetilde{X}+ \begin{bmatrix} \mathbf{Q}_{1}^{T}\mathbf{n}_{o} \\ \mathbf{Q}_{2}^{T}\mathbf{n}_{o} \end{bmatrix} ro​=[Q1​​Q2​​][TH​0​]X +no​⇒[Q1T​ro​Q2T​ro​​]=[TH​0​]X +[Q1T​no​Q2T​no​​]
由于 Q 2 T r o \mathbf{Q}_{2}^{T}\mathbf{r}_{o} Q2T​ro​只是噪声项，可以忽略，因此可以得到最终的观测方程如下:
r n = Q 1 T r o = T H X ~ + n n \mathbf{r}_{n}=\mathbf{Q}_{1}^{T}\mathbf{r}_{o}=\mathbf{T}_{H}\widetilde{X}+\mathbf{n}_{n} rn​=Q1T​ro​=TH​X +nn​
卡尔曼增益:
K = P T H T ( T H P T H T + R n ) − 1 \mathbf{K}=\mathbf{P}\mathbf{T}_{H}^{T}(\mathbf{T}_{H}\mathbf{P}\mathbf{T}_{H}^{T}+\mathbf{R}_{n})^{-1} K=PTHT​(TH​PTHT​+Rn​)−1
误差状态向量增量:
Δ X = K r n \Delta\mathbf{X}=\mathbf{K}\mathbf{r}_{n} ΔX=Krn​
协方差矩阵更新:
P k + 1 ∣ k + 1 = ( I ξ − K T H ) P k + 1 ∣ k ( I ξ − K T H ) T + K R n K T \mathbf{P}_{k+1|k+1}=(\mathbf{I}_{\xi}-\mathbf{KT}_{H})\mathbf{P} _{k+1|k}(\mathbf{I}_{\xi}-\mathbf{KT}_{H})^{T}+\mathbf{KR}_{n}\mathbf{K}^{T} Pk+1∣k+1​=(Iξ​−KTH​)Pk+1∣k​(Iξ​−KTH​)T+KRn​KT