2.1.

Pinhole Camera Model

This work used a pinhole camera model, whose details are shown in Fig. 1. The camera coordinate frame is defined by ${}^{C}z$ paralleling to the optical axis, ${}^{C}x$ and ${}^{C}y$ matching the horizontal and vertical image directions. The projection results of the feature $P_j$ on the image plane are expressed as

where $f$ is the focal length, ${[{\mu }_j,{\nu }_j]}^T$ is the projected pixel coordinates of the feature point, and ${{}^C\mathbf{P}}_j={[{{}^{C}x}_j,{}^C{y}_j,{{}^{C}z}_j]}^T$. With the previous assumption on the identity of the frame $\{B\}$ and $\{C\}$, the coordinate of the feature expressed in the camera frame is given as

When considering the flight environment for fixed-wing aircraft, which generally holds the flight height above hundreds of meters, flat terrain such as plain and farm field the topographic relief can be neglected compared to the flight height. In such scenes, we assume that the visual features lie in a level plane. Further, without loss of generality, by assuming the feature plane contains the origin of the global inertial frame, we obtained ${\mathbf{e}}_3^T{}^G{\mathbf{P}}_j=0$, where ${\mathbf{e}}_3={[\begin{array}{ccc}0& 0& 1\end{array}]}^T$. We also adopted the unit quaternion ${}_B\mathbf{q}_G=q_0\mathbf{i}+q_1\mathbf{j}+q_2\mathbf{k}+q_3$ to represent the rotation from the global coordinate frame $\{G\}$ to the body-fixed coordinate frame $\{B\}$ at the time $t$. Hence, ${}_{B}T_G$ can be replaced by $C({}_B\mathbf{q}_G)$ which describes the rotational matrix corresponding to ${}_B\mathbf{q}_G$. Thus, the scale of line-of-sight vector and also the depth component of ${}^C{\mathbf{P}}_j$, namely ${{}^{C}z}_j$ in Eq. (5) can be represented as

2.2.

Optical Flow Computation

After the characterization of the motion equation of the camera and pinhole camera model, we focused on the optical flow computation method. In this work, the optical flow was determined as the displacement between two successive images divided by the time interval between their capture. Based on the local brightness constancy assumption and small motion assumption, the block-matching algorithm approximates the image motion by a displacement, which yielded the best match between image regions. The concept of the procedure, shown in Fig. 2, as well as the specific algorithm for displacement computation outlined in Fig. 3, starts with distinct feature extraction and ends up with minimizing the SSD cost function among the search window,^9,17 and the result provided the displacement $\mathbf{d}({\mathbf{X}}_k)$ of the current feature, which can be formulated as

where $I_O(·)$ and $I_C(·)$ are both image intensity functions. Thus, the two-dimensional (2-D) optical flow $\dot{\zeta }$ can be obtained, i.e., $\dot{\mathbf{\zeta }}=\frac{\mathbf{d}({\mathbf{X}}_k)}{\mathrm{\Delta }t}$.

Furthermore, we proved that the error of optical flow computation based on the SSD block-matching algorithm is subject to a Gaussian distribution, which is not fully explained by most reviewed articles, such as Farid Kendoul’s work.⁹ Therefore, we redefined the whole algorithm as a maximum likelihood estimation problem. The displacement of the pixel can be seen as a warp function $\omega$, which transforms a pixel point ${\mathbf{p}}_k(x_k,y_k)$ in the original image frame ($I_O$) into a point ${\mathbf{p}}_k^{\prime }(x_k^{\prime },y_k^{\prime })$ in the current image frame ($I_C$) by a 2-D deformation vector $\mathbf{\zeta }$, namely ${\mathbf{p}}_k(x_k,y_k)\in I_O$, $\omega ({\mathbf{p}}_k,\mathbf{\zeta })={\mathbf{p}}_k^{\prime }(x_k^{\prime },y_k^{\prime })\in I_C$. Based on the local brightness constancy assumption, which is given as

without loss of generality, ${\epsilon }_k$ can be modeled as Gaussian noise as ${\epsilon }_k\sim \mathrm{N}(0,{\sigma }^2)$. Given the $b\times b$ size block of pixels, the problem aimed at the estimation of $\mathbf{\zeta }$ by a patch of pixel samples ${\{I_O({\mathbf{p}}_k)\}}_{k=1}^{n=b\times b}$. According to Eq. (9), $I_O({\mathbf{p}}_k)\sim N\{I_C[\omega ({\mathbf{p}}_k,\mathbf{\zeta })],{\sigma }^2\}$, thus the corresponding probability density function is provided as

Hence, we derived the likelihood function for the pixel samples as

where the log-likelihood can be written as in which, $n\log C$ only contains items irrelevant to $\mathbf{\zeta }$. The maximum likelihood estimator $\widehat{\mathbf{\zeta }}$ maximizes the log-likelihood, i.e., $\widehat{\mathbf{\zeta }}=\arg \underset{\mathbf{\zeta }}{\max }\log L(\mathbf{\zeta })$, which is equivalent to finding the optimal displacement value that minimizes the least square function according to Eqs. (7) and (8). Therefore, we obtained $\widehat{\mathbf{\zeta }}=\underset{-w\le dx,dy\le w}{\arg \min }\mathrm{SSD}({\mathbf{X}}_k,dx,dy)$.

The maximum likelihood estimate (MLE) is proven to be asymptotically normally distributed,^18,19 and the estimated covariance error of MLE is given by the inverse of the observed Fisher information matrix; therefore, the maximum likelihood estimator can be expressed as

where ${\mathbf{\zeta }}_0$ denotes the true displacement parameter, which can be obtained by the SSD block-matching algorithm. It is worth mentioning that in this context, the SSD block-matching algorithm is a brute force method that would guarantee returns with the global optimum. Matrix $J({\mathbf{\zeta }}_0)$ is the Fisher information for an individual observation, while $J({\widehat{\mathbf{\zeta }}}_{\mathrm{ML}})$ is the observed Fisher information matrix and is defined as which is also the expectation of the negative Hessian matrix of the log-likelihood function.¹⁸ Since the optical flow $\dot{\mathbf{\zeta }}$ was calculated with the displacement motion divided by interframe time, we saw that the optical flow error maintained a Gaussian distribution, which was the prerequisite for adopting the Kalman filter.

Next, we deduce the measurement equation of optical flow. The projected pixel coordinates of the feature point $P_j$ on the image plane are given by Eq. (4), taking the time-derivatives of both sides of Eq. (4) leads to the expression for optical flow velocity

Consider the relative motion between the camera and feature point by replacement of ${{}^C\dot{\mathbf{P}}}_j$ in Eq. (3) into Eq. (15), and eliminating $[{}^C{x}_j,{}^C{y}_j,{}^C{z}_j]$ with $[{\mu }_j,{\nu }_j,f]$ based on Eqs. (4) and (6), the optical flow equation can be computed as

where ${}^G\mathbf{v}$ is the absolute velocity of the aircraft in the global inertial frame, i.e., ${}^G\dot{\mathbf{r}}={}^G\mathbf{v}$, and ${\mathbf{\xi }}_j$ was modeled as zero mean, white Gaussian noise with covariance ${\mathbf{R}}_{{\mathbf{\xi }}_j}$ which is related to the inverse of the negative Hessian for log-likelihood function as shown in Eq. (14).

2.3.

IMU Measurement

The IMU outputs are measured quantities in the body-fixed frame, angular rate ${\mathbf{\omega }}_m$ and acceleration ${\mathbf{a}}_m$, which were modeled as^20,21

where $\mathbf{\omega }\in {\mathbb{R}}^3$ and $\mathbf{a}\in {\mathbb{R}}^3$ are the true value, which are corrupted by IMU biases ${\mathbf{b}}_{\mathbf{\omega }}$ and ${\mathbf{b}}_{\mathbf{a}}$, IMU drift noise ${\mathbf{n}}_{\mathbf{\omega }}$ and ${\mathbf{n}}_{\mathbf{a}}$ which assumed to be zero-mean Gaussian white noise. $\mathbf{g}$ is the gravitational acceleration. The IMU biases are nonstatic but rather are seen as a random walk process with the biases noise ${\mathbf{n}}_{\mathbf{b}\mathbf{\omega }}$ and $n_{\mathbf{ba}}$. All the noise terms $\mathbf{n}={[{\mathbf{n}}_{\mathbf{a}}^T,{\mathbf{n}}_{\mathbf{\omega }}^T,{\mathbf{n}}_{\mathbf{ba}}^T,{\mathbf{n}}_{\mathbf{b}\mathbf{\omega }}^T]}^T$ are specified by the corresponding scalar variance coefficient ${\sigma }_{{\mathbf{n}}_{\mathbf{a}}}^2$, ${\sigma }_{{\mathbf{n}}_{\mathbf{\omega }}}^2$, ${\sigma }_{{\mathbf{n}}_{\mathbf{ba}}}^2$, and ${\sigma }_{{\mathbf{n}}_{\mathbf{b}\mathbf{\omega }}}^2$; the diagonal covariance matrices are given by ${\mathbf{Q}}_{\mathbf{a}}={\sigma }_{{\mathbf{n}}_{\mathbf{a}}}^2{\mathbf{I}}_3$, ${\mathbf{Q}}_{\mathbf{\omega }}={\sigma }_{{\mathbf{n}}_{\mathbf{\omega }}}^2{\mathbf{I}}_3$, ${\mathbf{Q}}_{\mathbf{ba}}={\sigma }_{{\mathbf{n}}_{\mathbf{ba}}}^2{\mathbf{I}}_3$, and ${\mathbf{Q}}_{\mathbf{b}\mathbf{\omega }}={\sigma }_{{\mathbf{n}}_{\mathbf{b}\mathbf{\omega }}}^2{\mathbf{I}}_3$. According to Refs. 22 and 23, they can be obtained by the Allan variance technique.

3.1.

System State and Propagation

The estimator vector was the $16\times 1$ vector, and all the symbol definition can be found in Sec. 2.

The system kinematic equation that describes the time evolution of the state was similar to Hesch’s model,²¹ of which the corresponding linear discretized form of predicted state estimate is provided as

In order to derive the process noise covariance matrix, the error dynamic needed to be derived from the kinematic equation along its nominal form. We defined the error state in the estimate $\widehat{x}$ of a quantity $x$ for aircraft position, velocity, and IMU biases as $\delta x=x-\widehat{x}$. And it is exceptive for the quaternion error, which was defined by the $3\times 1$ angle-error vector $\delta \mathbf{\theta }$, as $\delta \mathbf{q}\triangleq {[\begin{array}{cc}\frac{1}{2}\delta {\mathbf{\theta }}^T& 1\end{array}]}^T$. Then, the linearized first-order approximation of continuous-time error-state kinematics^14,20,24 are provided as

In practical terms, the ESKF is realized in a time-discrete manner, with the update time interval $\mathrm{\Delta }t=t_{k+1}-t_k$, which was also seen as the sampling period of IMU signals. By using numerical integration of Eq. (20), we obtained the discrete error-state equation

where ${\mathbf{\Phi }}_k$ was the transition matrix and ${\mathbf{G}}_k$ denoted the perturbation matrix of noise vector ${\mathbf{n}}_k$. In the process of integration and discretization, based on Joan Sola’s results,²⁰ the following auxiliary series was introduced: by assigning the value of $n$ with $\{\mathrm{0,1},\mathrm{2,3}\}$, we obtained

Here, ${\mathbf{\Gamma }}_0^T$ means the incremental rotation matrix if rotating an arbitrary coordinate frame with a rotational rate of $-\mathbf{\omega }$ for timespan $\mathrm{\Delta }t$ according to the Rodrigues rotation formula; therefore, the discrete linearized error dynamic transition matrix ${\mathbf{\Phi }}_k$ as a function of above series²⁴ (for readability ${\widehat{\mathbf{\Gamma }}}_n^T={\mathbf{\Gamma }}_n^T[{\mathbf{\omega }}_m(k)-{\widehat{\mathbf{b}}}_{\mathbf{\omega }}(k)]$) can be written as in matrix

The discrete process noise covariance matrix ${\mathbf{Q}}_k$ was calculated as

Thus, the error covariance propagation resulted as

3.2.

Measurement Update Equation

According to Eq. (16), the optical flow measurement can be expressed by the estimated state vector. Since the aircraft angular rate was not part of the state vector, we replaced it with ${}^B\widehat{\mathbf{\omega }}={\mathbf{\omega }}_m(t)-{\widehat{\mathbf{b}}}_{\mathbf{\omega }}(t)$. Substituting the coefficient matrices associating with ${\mu }_j$, ${\nu }_j$, and $f$ in Eq. (16), and it would be clearer with notation listed as

Furthermore, by subtracting measured angular rate item and redefining the revised optical flow measurement ${\dot{\overline{\mathbf{\zeta }}}}_j$ as a new measurement of the filter, the right side was expressed only using the state vector $\widehat{\mathbf{x}}$; therefore, the following notation remains as

So that the measurement residual, also called innovation, was given as

where the Jacobian measurement was

Note that the residual noise ${\mathbf{\eta }}_k^j$ was involved with the optical flow algorithm error and gyroscope drift noise, i.e., ${\mathbf{\eta }}_k^j={\mathbf{\xi }}_k^j-B_{{\mathbf{P}}_{\mathbf{j}}}·{\mathbf{n}}_{\mathbf{\omega },k}$, hence, the covariance matrix of the revised optical flow measurement was the sum of the covariance of optical flow algorithm ${\mathbf{R}}_{{\mathbf{\xi }}_j}$ and the gyroscope covariance ${\mathbf{Q}}_{\mathbf{\omega }}$, which was given as

In general, there were a set of optical flow vectors measured at one time, stacking all the observation residuals of Eq. (31) into one vector, which yielded, ${\mathbf{y}}_k={\mathbf{H}}_k\delta {\mathbf{x}}_k+{\mathbf{\eta }}_k$, where ${\mathbf{y}}_k$, ${\mathbf{H}}_k$, and ${\mathbf{\eta }}_k$ were composed of the block elements ${\mathbf{y}}_k^j$, ${\mathbf{H}}_k^j$, and ${\mathbf{\eta }}_k^j$, such that the measurement covariance matrix could be stacked together diagonally as

Then, the a priori state estimate would be updated with current measurements according to the following steps: (1) calculate the Kalman gain as

and (2) calculate the observed error state correction as with these quantities, the nominal states and error covariance get updated as

3.3.

Error-State Kalman Filter Reset

For the ESKF reset process, we considered the expression among the new error $\delta {\widehat{\mathbf{x}}}_k^{+}$, the old error $\delta {\widehat{\mathbf{x}}}_k^{-}$, and the observed error correction $\mathrm{\Delta }{\mathbf{x}}_k$, as the true value was constant, yielding ${\widehat{\mathbf{x}}}_k^{+}+\delta {\widehat{\mathbf{x}}}_k^{+}={\widehat{\mathbf{x}}}_k^{-}+\delta {\widehat{\mathbf{x}}}_k^{-}$. Considering that the observed error has been injected into the updated nominal state, i.e., ${\widehat{\mathbf{x}}}_k^{+}={\widehat{\mathbf{x}}}_k^{-}+\mathrm{\Delta }{\mathbf{x}}_k$ so that $\delta {\widehat{\mathbf{x}}}_k^{+}=\delta {\widehat{\mathbf{x}}}_k^{-}-\mathrm{\Delta }{\mathbf{x}}_k$, we defined an error reset function $\delta \mathbf{x}\triangleq g(\delta \mathbf{x})=\delta {\widehat{\mathbf{x}}}_k^{-}-\mathrm{\Delta }{\mathbf{x}}_k$, where the reset operation was to ensure the error would reset $\delta \mathbf{x}\leftarrow 0$, meanwhile, the covariance of error needed to be modified as well,¹⁴ thus leading to

where $G$ represented the Jacobian matrix according to Joan Sola’s proof¹⁵ defined as