MSCKF学习笔记

估计器描述

EKF的状态向量结构

由向量描述的$\mathrm{IMU}$状态演变：
$${\mathbf{X}}_{\mathrm{IMU}}={\left[\begin{array}{lllll}{}_G^I{\bar{q}}^T& {\mathbf{b}}_g{}^T& G_{{\mathbf{v}}_I}^T& {\mathbf{b}}_a{}^T& {}^G{\mathbf{p}}_I^T\end{array}\right]}^T$$
IMU误差状态定义为：
$${\tilde{\mathbf{X}}}_{\mathrm{IMU}}={\left[\begin{array}{ccccc}\delta {\theta }_I^T& {\tilde{\mathbf{b}}}_g^T& G{\tilde{\mathbf{v}}}_I^T& {\tilde{\mathbf{b}}}_a^T& {}^G{\tilde{\mathbf{p}}}_I^T\end{array}\right]}^T$$
其中，在位置、速度和偏置的估计中，采用标准的加性误差定义。方向误差由四元数乘法定义。

四元数误差为：
$$\delta \bar{q}\simeq {\left[\begin{array}{cc}\frac{1}{2}\delta {\theta }^T& 1\end{array}\right]}^T$$
这里使用$\delta \theta$来表示最小姿态误差

假设$N$个相机姿态包括在时间步长$k$的EKF状态向量中，该向量具有以下形式:
$${\hat{\mathbf{X}}}_k={\left[\begin{array}{llllll}{\hat{\mathbf{X}}}_{{\mathrm{IMU}}_k}^T& {}_G^{C_1}{\hat{\bar{q}}}^T& {}^G{\hat{\mathbf{p}}}_{C_1}^T& \cdots & {}_G^{C_N}{\hat{\bar{q}}}^T& {}^G{\hat{\mathbf{p}}}_{C_N}^T\end{array}\right]}^T$$
其中${}_G^{C_1}{\hat{\bar{q}}}^T$和${}^G{\hat{\mathbf{p}}}_{C_1}^T$，$i=\mathrm{1...}N$分别是摄像机姿态和位置的估计值。相应地定义了EKF误差状态向量:
$${\tilde{\mathbf{X}}}_k={\left[\begin{array}{llllll}{\tilde{\mathbf{X}}}_{{\mathrm{IMU}}_k}^T& \mathbf{\delta }{\mathbf{\theta }}_{C_1}^T& {}^G{\tilde{\mathbf{p}}}_{C_1}^T& \dots & \mathbf{\delta }{\mathbf{\theta }}_{C_N}^T& {}^G{\tilde{\mathbf{p}}}_{C_N}^T\end{array}\right]}^T$$

传播

连续时间系统模型

IMU状态的时间演化描述为：
$$\begin{array}{c}{}_G^I\dot{\bar{q}}(t)=\frac{1}{2}\mathbf{\Omega }(\mathbf{\omega }(t){)}_G^I\bar{q}(t),{\dot{\mathbf{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{array}$$
在这些表达式中，${}^G\mathbf{a}$ 是全局坐标系中的物体加速度，$ω = [ω_x\quadω_y\quadω_z]^T $是 $\mathrm{IMU}$ 坐标系中表示的旋转速度.

陀螺仪和加速度计测量值 ${\omega }_m$ 和 $a_m$ 分别由下式给出:
$$\begin{array}{rl}& {\omega }_m=\mathbf{\omega }+\mathbf{C}{(}_G^I\bar{q}){\mathbf{\omega }}_G+{\mathbf{b}}_g+{\mathbf{n}}_g\\ & a_m=\mathbf{C}{(}_G^I\bar{q}){(}^G\mathbf{a}-{}^G\mathbf{g}{+2\lfloor {\omega }_{\mathbf{G}}\times \rfloor }^G{\mathbf{v}}_I+\lfloor {\omega }_G\times {\rfloor }^2{}^G{\mathbf{p}}_I)+{\mathbf{b}}_a+{\mathbf{n}}_a\end{array}$$
其中$\mathbf{C}(\cdot )$表示旋转矩阵，${\mathbf{n}}_g$和$\mathbf{n}_a $是零均值高斯白噪声过程，用于模拟测量噪声。值得注意的是，${\mathrm{IMU}}$测量包含了行星旋转的影响，此外，加速度计测量包含了重力加速度，${^G\mathbf{g}}$，以本地坐标系表示。

在状态传播方程中应用期望算子。获得用于传播演化$\mathrm{IMU}$状态的估计的方程:
$$\begin{array}{c}{}_G^I\dot{\bar{q}}=\frac{1}{2}\mathbf{\Omega }(\hat{\omega }{)}_G^I\hat{\bar{q}},{\hat{\mathbf{b}}}_g=0_{3\times 1},\\ {}^G{\dot{\hat{\mathbf{v}}}}_I={\mathbf{C}}_{\hat{q}}^T\hat{\mathbf{a}}-2\lfloor {\mathbf{\omega }}_G\times \rfloor {}^G{\hat{\mathbf{v}}}_I-\lfloor {\mathbf{\omega }}_G\times \rfloor {}^2{}^G{\hat{\mathbf{p}}}_I+{}^G\mathbf{g}\\ {\dot{\hat{\mathbf{b}}}}_a=0_{3\times 1},{}^G{\dot{\hat{\mathbf{p}}}}_I={}^G{\hat{\mathbf{v}}}_I\end{array}$$
为简单起见，用$\mathbf{C}{\hat{q}}= \mathbf{C}({G}^{I}\hat{\bar{q}}) \$，${\hat{\mathbf{a}}}=\mathbf{a}{m}-\mathbf{b}{a} $和$\hat{\omega}=\omega_m-\hat{\textbf{b}}g-\textbf{C}{\hat{q}}\omega_G$表示，IMU误差状态的线性化连续时间模型为:
$${\dot{\tilde{\mathbf{X}}}}_{\mathrm{IMU}}=\mathbf{F}{\tilde{\mathbf{X}}}_{\mathrm{IMU}}+\mathbf{G}{\mathbf{n}}_{\mathrm{IMU}}$$
其中${\mathbf{n}}_{\mathrm{IMU}}={\left[\begin{array}{cccc}{\mathbf{n}}_g^T& {\mathbf{n}}_{wg}^T& {\mathbf{n}}_a^T& {\mathbf{n}}_{wa}^T\end{array}\right]}^T$为系统噪声。${\mathbf{n}}_{\mathrm{IMU}}$的协方差矩阵${\mathbf{Q}}_{\mathrm{IMU}}$取决于$\mathrm{IMU}$的噪声特性，在传感器标定时离线计算。最后，方程中出现的矩阵$\mathbf{F}$和$\mathbf{G}$由下式给出:
$$\mathbf{F}=\left[\begin{array}{ccccc}-\lfloor \hat{\mathbf{\omega }}\times \rfloor & -{\mathbf{I}}_3& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ -{\mathbf{C}}_{\hat{q}}^T\lfloor \hat{\mathbf{a}}\times \rfloor & 0_{3\times 3}& -2\lfloor {\omega }_C\times \rfloor & -{\mathbf{C}}_{\hat{q}}^T& -\lfloor {\omega }_G\times {\rfloor }^2\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& {\mathbf{I}}_3& 0_{3\times 3}& 0_{3\times 3}\end{array}\right]$$
其中${\mathbf{I}}_3$是$3\times 3$单位矩阵，那么
$$\text{G}=\left[\begin{array}{cccc}-{\text{I}}_3& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}\\ {\text{0}}_{3\times 3}& {\text{I}}_3& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}\\ {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& -{\text{C}}_{\hat{q}}^T& {\text{0}}_{3\times 3}\\ {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{I}}_{3\times 3}\\ {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}\end{array}\right]$$

离散时间实现

$\mathrm{IMU}$ 以周期 $T$ 对信号 ${\omega }_m$ 和 ${\mathbf{a}}_m$ 进行采样，这些测量值用于 $\mathrm{IMU}$ 中的状态传播。每次接收到新的 $\mathrm{IMU}$ 测量值时，$\mathrm{IMU}$状态估计都会使用方程式的 5 阶 $Runge-Kutta$ 数值积分进行传播。 此外，$\mathrm{EFK}$ 协方差矩阵需要进行传播。为此，引入以下协方差划分：
$${\mathbf{P}}_{k|k}=\left[\begin{array}{cc}{\mathbf{P}}_{I{I}_{k|k}}& {\mathbf{P}}_{I{C}_{k|k}}\\ {\mathbf{P}}_{I{C}_{k|k}}^T& {\mathbf{P}}_{C{C}_{k|k}}\end{array}\right]$$
其中${\mathbf{P}}_{I{I}_{k|k}}$为 $\mathrm{IMU}$ 状态演化的$15\times 15$协方差矩阵，${\mathbf{P}}_{C{C}_{k\mid k}}$为相机姿态估计的$6N\times 6N$协方差矩阵，${\mathbf{P}}_{I{C}_{k|k}}$为$\mathrm{IMU}$状态误差与相机姿态估计的相关性。用这种表示法，传播状态的协方差矩阵为:
$${\mathbf{P}}_{k+1\mid k}=\left[\begin{array}{cc}{\mathbf{P}}_{I{I}_{k+1\mid k}}& \mathbf{\Phi }(t_k+T,t_k){\mathbf{P}}_{I{C}_{k\mid k}}\\ {\mathbf{P}}_{I{C}_{k\mid k}}^T\mathbf{\Phi }(t_k+T,t_k{)}^T& {\mathbf{P}}_{C{C}_{k\mid k}}\end{array}\right]$$
其中 ${\mathbf{P}}_{I{I}_{k+1|k}}$ 是通过 $Lyapunov$ 方程的数值积分计算的：
$${\dot{\mathbf{P}}}_{II}=\mathbf{F}{\mathbf{P}}_{II}+{\mathbf{P}}_{II}{\mathbf{F}}^T+\mathbf{G}{\mathbf{Q}}_{\mathrm{IMU}}{\mathbf{G}}^T$$
对时间区间$(t_k,t_k+T)$进行数值积分，初始条件为${\mathbf{P}}_{I{I}_{k|k}}$。状态转移矩阵$\mathbf{\Phi }(t_k+T,t_k)$同样通过微分方程的数值积分计算：
$$\dot{\mathbf{\Phi }}(t_k+\tau ,t_k)=\mathbf{F}\Phi (t_k+\tau ,t_k),\tau \in [0,T]$$
初始条件$\mathbf{\Phi }(t_k,t_k)={\mathbf{I}}_{15}$。

状态扩充

在记录新图像时，从$\mathrm{IMU}$姿态估计中计算相机姿态估计为:
$${}_G^C\hat{\bar{q}}={}_I^C\bar{q}\otimes {}_G^I\hat{\bar{q}},\text{ and }{}^G{\hat{\mathbf{p}}}_C={}^G{\hat{\mathbf{p}}}_I+{\mathbf{C}}_{\hat{q}}^T{}^I{\mathbf{p}}_C$$
其中${ }_I^C \bar{q} $是表示${\mathrm{IMU}}$和相机帧之间旋转的四元数，$^I \mathbf{p}_C$是相机帧的原点相对于$\left{I\right}$的位置，这两个都是已知的。将摄像机姿态估计值附加到状态向量上，对${\mathrm{EFK}}$的协方差矩阵进行相应的扩充-:
$${\mathbf{P}}_{k|k}\leftarrow \left[\begin{array}{c}{\mathbf{I}}_{6N+15}\\ \mathbf{J}\end{array}\right]{\mathbf{P}}_{k|k}{\left[\begin{array}{c}{\mathbf{I}}_{6N+15}\\ \mathbf{J}\end{array}\right]}^T$$
其中雅可比矩阵$\mathbf{J}$为：
$$\mathbf{J}=\left[\begin{array}{cccc}\mathbf{C}\left(\begin{array}{c}{}_I^C\bar{q}\end{array}\right)& 0_{3\times 9}& 0_{3\times 3}& 0_{3\times 6N}\\ \lfloor {\mathbf{C}}_{\bar{q}}^{TI}\mathbf{p}C\times \rfloor & 0_{3\times 9}& {\mathbf{I}}_3& 0_{3\times 6N}\end{array}\right]$$

测量模型

由于 $\mathrm{EFK}$ 用于状态估计，因此为了构建测量模型，只需定义一个与状态误差$\tilde{\text{X}}$线性相关的残差$\text{r}$，其一般形式为:
$$\text{r}=\text{H}\tilde{\text{X}}+\text{noise}$$
在该表达式中，$\text{H}$是测量雅可比矩阵，并且噪声项必须是零均值、白色并且与状态误差无关，才能应用$\text{EFK}$框架。

通过考虑单个特征$f_j$的情况，并从一组$M_j$个相机位姿${(}_G^{C_i}\bar{q},{}^G{\text{p}}_{C_i})$，其中$i\in {\mathcal{S}}_j$观察到该特征，提出了测量模型。该特征的每个$M_j$观测结果由以下模型描述：
$${\text{z}}_i^{(j)}=\frac{1}{{}^{C_i}{Z}_j}\left[\begin{array}{c}{}^{C_i}{X}_j\\ {}^{C_i}{Y}_j\end{array}\right]+{\text{n}}_i^{(j)},i\in {\mathcal{S}}_j$$
式中${\text{n}}_i^{(j)}$$2\times 1$图像噪声向量，协方差矩阵${\mathbf{R}}_i^{(j)}={\sigma }_{\mathrm{im}}^2{\mathbf{I}}_2$。特征位置在相机帧${}^{C_i}{\mathbf{p}}_{f_j}$中表示为:
$${}^{C_i}{\text{p}}_{f_j}=\left[\begin{array}{c}{}^{C_i}{X}_j\\ {}^{C_i}{Y}_j\\ {}^{C_i}{Z}_j\end{array}\right]=\text{C}{(}_G^{C_i}\bar{q}){(}^G{\text{p}}_{f_j}-{{}^G\text{p}}_{C_i})$$
其中${}^G{\mathbf{p}}_{f_j}$是全局框架中的$\text{3D}$特征位置。由于这是未知的，在我们算法的第一步中，我们使用最小二乘最小化来获得特征位置的估计，${}^G{\hat{\mathbf{p}}}_{f_j}$。

这是通过测量${\mathbf{z}}_i^{(j)},i\in {\mathcal{S}}_j,$以及相应时刻相机姿势的滤波器估计来实现的。
$${\mathbf{r}}_i^{(j)}={\mathbf{z}}_i^{(j)}-{\hat{\mathbf{z}}}_i^{(j)}$$
这里
$${\hat{\mathbf{z}}}_i^{(j)}=\frac{1}{{}^{C_i}{\hat{Z}}_j}\left[\begin{array}{c}{}^{C_i}{\hat{X}}_j\\ {}^{C_i}{\hat{Y}}_j\end{array}\right],\left[\begin{array}{c}{}^{C_i}{\hat{X}}_j\\ {}^{C_i}{\hat{Y}}_j\\ {}^{C_i}{\hat{Z}}_j\end{array}\right]=\mathbf{C}\left({}_G^{C_i}\hat{\bar{q}}\right)\left({}^G{\hat{\mathbf{p}}}_{f_j}-{}^G{\hat{\mathbf{p}}}_{C_i}\right)$$
对相机姿态和特征位置的估计进行线性化处理，残差可以近似为:
$${\mathbf{r}}_i^{(j)}\simeq {\mathbf{H}}_{{\mathbf{X}}_i}^{(j)}\tilde{\mathbf{X}}+{\mathbf{H}}_{f_i}^{(j)}{}^G{\tilde{\mathbf{p}}}_{f_j}+{\mathbf{n}}_i^{(j)}$$
在前面的表达式中，${\mathbf{H}}_{{\mathbf{X}}_i}^{(j)}$和${\mathbf{H}}_{f_i}^{(j)}$分别是关于状态和特征位置的测量${\mathbf{z}}_i^{(j)}$的雅可比矩阵，${}^G{\tilde{\mathbf{p}}}_{f_j}$是$f_j$的位置估计的误差。

通过叠加该特征的所有$M_j$测量值的残差，得到:
$${\mathbf{r}}^{(j)}\simeq {\mathbf{H}}_{\mathbf{X}}^{(j)}\tilde{\mathbf{X}}+{\mathbf{H}}_f^{(j)}{}^G{\tilde{\mathbf{p}}}_f+{\mathbf{n}}^{(j)}$$
其中 ${\mathbf{r}}^{(j)},{\mathbf{H}}_{\mathbf{X}}^{(j)},{\mathbf{H}}_f^{(j)},$和${\mathbf{n}}^{(j)}$是包含元素 ${\mathbf{r}}_i^{(j)},{\mathbf{H}}_{{\mathbf{X}}_i}^{(j)},{\mathbf{H}}_{f_i}^{(j)},$和${\mathbf{n}}_i^{(j)}$的分块向量或矩阵${\mathbf{n}}^{(j)}$$,i\in {\mathcal{S}}_j$。由于不同图像中的特征观测值是独立的，${\mathbf{n}}^{(j)}$的协方差矩阵为${\mathbf{R}}^{(j)}={\sigma }_{\mathrm{im}}^2{\mathbf{I}}_{2M_i}$。

请注意，由于状态估计$\mathbf{X}$被用于计算特征位置估计，因此方程中的误差${}^G{\tilde{\mathbf{p}}}_f$与误差$\tilde{\mathbf{X}}$相关。因此，残差${\mathbf{r}}^{(j)}$不能直接应用于$\mathrm{EFK}$中的测量更新。为了解决这个问题，定义一个残差${\mathbf{r}}_o^{(j)}$，通过将${\mathbf{r}}^{(j)}$投影到矩阵${\mathbf{H}}_f^{(j)}$的左零空间上得到。具体来说，如果让$\mathbf{A}$表示由${\mathbf{H}}_f$的左零空间的基向量形成的列组成的酉矩阵，可以得到：
$$\begin{array}{rl}{\mathbf{r}}_o^{(j)}& ={\mathbf{A}}^T({\mathbf{z}}^{(j)}-{\hat{\mathbf{z}}}^{(j)})\simeq {\mathbf{A}}^T{\mathbf{H}}_{\mathbf{X}}^{(j)}\tilde{\mathbf{X}}+{\mathbf{A}}^T{\mathbf{n}}^{(j)}\\ & ={\mathbf{H}}_o^{(j)}{\tilde{\mathbf{X}}}^{(j)}+{\mathbf{n}}_o^{(j)}\end{array}$$
该残差与特征坐标中的误差无关，因此可以基于它进行 $\mathrm{EFK}$ 更新。上式定义了观察到特征$f_j$的所有相机姿势之间的线性化约束。这表达了测量${\text{z}}_i^{(j)}$为$M_j$状态提供的所有可用信息，因此所得到的$\mathrm{EFK}$更新是最优的，除了线性化引起的不准确性。

由于矩阵A是酉的，因此噪声向量${\mathbf{n}}_o^{(j)}$的协方差矩阵为:
E { n o ( j ) n o ( j ) T } = σ i m 2 A T A = σ i m 2 I 2 M j − 3 E\{\mathbf{n}_o^{(j)}\mathbf{n}_o^{(j)T}\}=\sigma_{\mathrm{im}}^2\mathbf{A}^T\mathbf{A}=\sigma_{\mathrm{im}}^2\mathbf{I}_{2M_j-3} E{no(j)​no(j)T​}=σim2​ATA=σim2​I2Mj​−3​

更新EKF

$\mathrm{EFK}$更新由以下两个事件之一触发：

• 当在许多图像中跟踪的特征不再被检测到时。这种情况最常见，因为特征移动到摄像机的视野之外。
• 每次记录新图像时，当前相机姿态估计的副本都包含在状态向量中。如果达到了允许的最大相机姿态数$N_{\max }$，那么至少要删除其中一个旧的姿态。在丢弃状态之前，使用发生在相应时间点处的所有特征观测，以利用它们的本地化信息。在算法中，选择$N_{\max }/3$个姿态，这些姿态在时间上均匀分布，从第二旧的姿态开始。在使用这些姿态共同的特征约束进行$\mathrm{EFK}$更新之后，这些姿态将被丢弃。选择始终保留状态向量中最古老的姿态，因为涉及到更早的姿态的几何约束通常对应于更大的基线，因此具有更有价值的定位信息。这种方法在实践中表现非常出色。

以下是详细讨论更新过程。考虑在给定时间步骤中，必须处理上述两个标准选择的$L$个特征的约束。按照前面一节中描述的过程，计算每个特征的残差向量${\text{r}}_o^{(j)},j=1\dots L,$以及相应的测量矩阵${\text{H}}_o^{(j)},j=1\dots L。$通过将所有残差堆叠在一个单独的向量中，得到：
$${\mathbf{r}}_o={\mathbf{H}}_{\mathbf{X}}\tilde{\mathbf{X}}+{\mathbf{n}}_o$$
其中，${\mathbf{r}}_o$和${\mathbf{n}}_o$分别是具有分块元素${\text{r}}_o^{(j)}$和${\text{n}}_o^{(j)},j=1\dots L,$的向量，${\mathbf{H}}_{\mathbf{X}}$是具有分块行${\text{H}}_{\mathbf{X}}^{(j)},j=1\dots L$的矩阵。

由于特征测量值在统计上是独立的，因此噪声向量${\text{n}}_o^{(j)}$是不相关的。

因此，噪声向量${\text{n}}_o$的协方差矩阵为${\text{R}}_o={\sigma }_{\text{im}}^2{\text{I}}_d,$其中$d={\sum }_{j=1}^L(2M_j-3)$为残差${\text{R}}_o$的维数。在实践中出现的一个问题是$d$可能是一个相当大的数字。

为了降低$\mathrm{EFK}$更新的计算复杂度，采用矩阵${\mathbf{H}}_{\mathbf{X}}$的$\mathbf{QR}$分解。具体地说，把这个分解表示为：
$${\mathbf{H}}_{\mathbf{X}}=\left[\begin{array}{cc}{\mathbf{Q}}_1& {\mathbf{Q}}_2\end{array}\right]\left[\begin{array}{c}{\mathbf{T}}_{\mathbf{H}}\\ 0\end{array}\right]$$
式中${\mathbf{Q}}_1$和${\mathbf{Q}}_2$为酉矩阵，其列分别构成${\mathbf{H}}_{\mathbf{X}}$的值域和零空间的基，${\mathbf{T}}_{\mathbf{H}}$为上三角矩阵。根据这个定义，得到:
$$\begin{array}{rl}{\mathbf{r}}_o& =\left[\begin{array}{cc}{\mathbf{Q}}_1& {\mathbf{Q}}_2\end{array}\right]\left[\begin{array}{c}{\mathbf{T}}_H\\ 0\end{array}\right]\tilde{\mathbf{X}}+{\mathbf{n}}_o\Rightarrow \\ \left[\begin{array}{c}{\text{Q}}_1^T{\text{r}}_o\\ {\text{Q}}_2^T{\text{r}}_o\end{array}\right]& =\left[\begin{array}{c}{\mathbf{T}}_H\\ 0\end{array}\right]\tilde{\mathbf{X}}+\left[\begin{array}{c}{\mathbf{Q}}_1^T{\mathbf{n}}_o\\ {\mathbf{Q}}_2^T{\mathbf{n}}_o\end{array}\right]\end{array}$$
从上一个方程可以清楚地看出，通过将残差${\mathbf{r}}_o$投影到${\mathbf{H}}_{\mathbf{X}}$值域的基向量上，保留了测量中的所有有用信息。剩余的${\mathbf{Q}}_2^T{\mathbf{r}}_o$只是噪声，可以完全丢弃。因此，使用以下残差来更新$\mathrm{EFK}$:
$${\mathbf{r}}_n={\mathbf{Q}}_1^T{\mathbf{r}}_o={\mathbf{T}}_H\tilde{\mathbf{X}}+{\mathbf{n}}_n$$
式中${\mathbf{n}}_n={\mathbf{Q}}_1^T{\mathbf{n}}_o$为噪声向量，其协方差矩阵为${\mathbf{R}}_n={\mathbf{Q}}_1^T{\mathbf{R}}_o{\mathbf{Q}}_1={\sigma }_{\mathrm{im}}^2{\mathbf{I}}_r,$,$r$为${\mathbf{Q}}_1$的列数。$\mathrm{EFK}$更新通过计算卡尔曼增益进行:
$$\mathbf{K}={\mathbf{PT}}_H^T{\left({\mathbf{T}}_H{\mathbf{PT}}_H^T+{\mathbf{R}}_n\right)}^{-1}$$
而对状态的校正由向量给出
$$\Delta \mathbf{X}=\mathbf{K}{\mathbf{r}}_n$$
最后，根据下式更新状态协方差矩阵:
$${\mathbf{P}}_{k+1\mid k+1}=\left({\mathbf{I}}_{\xi }-\mathbf{K}{\mathbf{T}}_H\right){\mathbf{P}}_{k+1\mid k}{\left({\mathbf{I}}_{\xi }-\mathbf{K}{\mathbf{T}}_H\right)}^T+{\mathbf{KR}}_n{\mathbf{K}}^T$$
其中$\xi =6N+15$是协方差矩阵的维数。