Measurement model and observability analysis for optical flow-aided inertial navigation

Abstract. We introduce a navigation filter fused with information of visual optical flow and data collected from an inertial measurement unit during GPS signal degradation. Under the assumption that the tracked feature points are located on a level plane, the feature depth can be explicitly expressed and an exact measurement model was derived. Moreover, an error model analysis for a block-matching-based optical flow algorithm has been investigated. The measurement error follows a Gaussian distribution, which is a prerequisite for leveraging the error-state Kalman filter. Subsequently, a local observability analysis of the proposed filter was performed yielding the expression of three unobservable directions. We emphasize the ability of the proposed filter to estimate the aircraft’s state, especially for accurate altitude estimation, without any help of prior knowledge or extra sensors. Finally, an extensive Monte Carlo analysis was used to verify the findings in the observability results showing that all states can be estimated except the absolute horizontal positions and rotation around the gravity vector. The effectiveness of the proposed filter is demonstrated through experimental hardware used to acquire outdoor flight test data.


Introduction
2][3][4] The traditional GPS/inertial navigation system (INS) may face severe drift and inaccuracy for the estimation, especially in environments where GPS is not reliable.Integrated inertial systems using sensors such as radar, LiDAR, or sonar can be regarded as a possible method to improve accuracy. 5However, radar or LiDAR may increase the cost, weight, and power consumption for an airborne platform, while low-cost sonar always shows limited performance for flights in outdoor scenes.Camera-aided navigation and visual odometry, on the other hand, provide rich information about the environment and the camera pose, which seems to be a superior solution to the above problems.
In recent years, simultaneous localization and mapping gathered a lot of attention since it can provide accurate and real-time estimates of six-dimensional parameters.Nevertheless, it is subject to a series of strict requirements such as high-quality images for feature extraction, hardware with powerful onboard processor needed as computational complexity increase related to the quantity of features, and repeated tracking for the same image features over longer periods of time is needed to realize loop-closure. 6Moreover, there are few applications for fixed-wing aircraft that are faster and higher than quadcopters in an outdoor unstructured environment.Optical flow-aided INS evades the need for mapping and landmark data and shows better tolerance in terms of handling scenes with less salient features and little contrast texture.In addition, it has a compact image representation and does not require the extra burden for image storage without loss of rich navigation information on the translation and rotational velocity as well as the scene depth. 7It is more suitable for implementation in systems with limited resources.Previous tests have been conducted to compare the performance between different optical flow algorithms for UAV navigation, as introduced in Ref. 8. The optical flow observation error is usually modeled with the default zero mean normal distribution without any explanations. 9We turn to computer vision scientists for a better answer, but it was found that they mainly focus on the evaluation methodology to give new statistics for indicating the performance. 10They have raised definitions such as "absolute flow endpoint error" and "frame interpolation error," 11 rather than proposing a specific model analysis for the flow error.Furthermore, the use of INS aided with optical flow generated by a single camera often adopts an additional ultrasound range sensor to measure the distance to the ground, 12 which solves the ambiguity of the scale factor between the translational velocity magnitude and the scene depth. 9Another commonly used method assumes that the specific maneuvers for UAV could be designed to keep a constant height or attitude.Five optimization constraintsbased methods for the estimation of ego-motion were presented in Florian Raudies' review, 10 which can successively solve the problem.As an example, Thomas P. Webb 13 leveraged epipolar constraint of feature points as the measurement equation in an implicit extended Kalman filter, also termed the essential or coplanarity constraint, and optical flow subspace constraint is similar to its differential version.However, the filter does not perform well in estimating the velocity states as the epipolar constraint weakly relates to velocity.A bilinear constraint-based visual-inertial scheme for relative motion estimation has been introduced. 7The observability analysis of INS errors for explicit measurements of a subspace constraint-based optical flow method was investigated in Jacques Waldmann's work. 14These approaches require sufficient numbers of optical flow features to obtain the corresponding linear equations for the unknowns.Xu et al. 2 presented an autonomous landing method using planar terrain features tracked in sequential images.Using the assumption of a level plane, therefore allowing for the scale of the line-of-sight vector to be expressed explicitly.
Compared to existing work, the main contributions of the proposed research focused on (1) a detailed error model analysis for optical flow measurement based on blockmatching algorithm was investigated and showed to obey a Gaussian distribution and therefore suitable for error-state Kalman filter (ESKF); (2) the feature depth was obtained without extra-measurement device through the assumption of flat terrain, which could be reasonably used for outdoor navigation missions when UAVs fly at a high altitude.In the flat terrain assumption, the height proved to be observable by local observability analysis, which was further verified by simulation and experimental results.In addition, a more detailed development of the ESKF is proposed, which fused the information from IMU and optical flow originally described in Refs.15 and 16.The paper is organized as follows: Sec. 2 describes the problem formulation, introduces the adopted sensors' dynamical model, reviews the computation of an optical flow block-matching algorithm, and derives the measurement error analysis.Section 3 outlines the structure of the ESKF to estimate the full states.Section 4 presents the observability analysis.Sections 5 and 6 show the simulation, experimental results, and corresponding discussion; the final conclusions are given in Sec. 7.

Problem Formulation
The geometry of the general case of an aircraft equipped with an IMU and a monocular camera is shown in Fig. 1.Symbol fBg represents the aircraft body-fixed frame, and the vehicle relative position to the global inertial reference frame fGg is denoted by the vector G r.The camera frame fCg is fixed to the aircraft with a mounting displacement B Δ. We denote the coordinates of the j'th feature point P j in the frame fGg by G P j , while C P j denotes the relative position of P j w.r.t.
the camera frame fCg.The transformation matrices B T G and C T B describe the rotations from fGg to fBg and fBg to fCg, respectively, where C T B is dependent on the mounting of the camera to the aircraft.The relative position of P j can be expressed in the camera frame fCg as E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 0 1 ; 3 2 6 ; 6 9 7 Considering the differential equation with respect to the reference frame fGg would require calculating the derivative of the rotation transformation matrix.Therefore, the rotational velocities of the vehicle expressed in the frame fBg and fCg are represented by B ω and C ω, respectively, resulting as E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 0 2 ; 3 2 6 ; 5 9 8 where G _ P j ¼ 0 derives from the assumption for static feature point.Consider a strapdown camera that can be assumed to have a fixed location coinciding with the center of gravity of the aircraft, so that fBg and fCg can be regarded as the same frame, i.e., B Δ ¼ B _ Δ ¼ 0, and C T B is the identity matrix, which yields the simplified Eq. ( 3):

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 E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 0 4 ; 3 2 6 ; 3 6 2 where f is the focal length, ½μ j ; ν j T is the projected pixel coordinates of the feature point, and C P j ¼ ½ C x j ; C y j ; C z j T .With the previous assumption on the identity of the frame fBg and fCg, the coordinate of the feature expressed in the camera frame is given as ; t e m p : i n t r a l i n k -; e 0 0 5 ; 3 2 6 ; 2 5 8 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 e T 3 G P j ¼ 0, where e 3 ¼ ½ 0 0 1 T .We also adopted the unit quaternion represent the rotation from the global coordinate frame fGg to the body-fixed coordinate frame fBg at the time t.Hence, B T G can be replaced by Cð B q G Þ which describes the rotational matrix corresponding to B q G .Thus, the scale of line-of-sight vector and also the depth component of C P j , namely C z j in Eq. ( 5) can be represented as E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 0 6 ; 6 3 ; 7 0 6

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 dðX k Þ of the current feature, which can be formulated as ; t e m p : i n t r a l i n k -; e 0 0 7 ; 6 3 ; 4 7 2 SSDðX k ; dx; dyÞ Δ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. 9Therefore, we redefined the whole algorithm as a maximum likelihood estimation problem.The displacement of the pixel can be seen as a warp function ω, which transforms a pixel point p k ðx k ; y k Þ in the original image frame Based on the local brightness constancy assumption, which is given as E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 0 9 ; 3 2 6 ; 4 6 9 without loss of generality, ε k can be modeled as Gaussian noise as Hence, we derived the likelihood function for the pixel samples as E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 1 1 ; 3 2 6 ; 2 8 9 where the log-likelihood can be written as ; t e m p : i n t r a l i n k -; e 0 1 2 ; 3 2 6 ; 2 0 8 log   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 ζ 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ðζ 0 Þ is the Fisher information for an individual observation, while Jð ζ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. 18Since the optical flow _ ζ 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 timederivatives of both sides of Eq. ( 4) leads to the expression for optical flow velocity E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 1 5 ; 6 3 ; 4 0 3 Consider the relative motion between the camera and feature point by replacement of C _ P j in Eq. (3) into Eq.( 15), and eliminating ½ C x j ; C y j ; C z j with ½μ j ; ν j ; f based on Eqs. ( 4) and ( 6), the optical flow equation can be computed as where G v is the absolute velocity of the aircraft in the global inertial frame, i.e., G _ r ¼ G v, and ξ j was modeled as zero mean, white Gaussian noise with covariance R ξ j which is related to the inverse of the negative Hessian for loglikelihood function as shown in Eq. ( 14).

IMU Measurement
The IMU outputs are measured quantities in the body-fixed frame, angular rate ω m and acceleration a m , which were modeled as 20,21 E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 1 7 ; 3 2 6 ; where ω ∈ R 3 and a ∈ R 3 are the true value, which are corrupted by IMU biases b ω and b a , IMU drift noise n ω and n a which assumed to be zero-mean Gaussian white noise.g is the gravitational acceleration.The IMU biases are nonstatic but rather are seen as a random walk process with the biases noise n bω and n ba .All the noise terms n ¼ ½n T a ; n T ω ; n T ba ; n T bω T are specified by the corresponding scalar variance coefficient σ 2 n a , σ 2 n ω , σ 2 n ba , and σ 2 n bω ; the diagonal covariance matrices are given by n ba I 3 , and Q bω ¼ σ 2 n bω I 3 .According to Refs.22 and 23, they can be obtained by the Allan variance technique.

Estimator Description
With the purpose to integrate the two different sources of information collected by IMU and monocular-camera, an ESKF was designed.While the IMU information served to make predictions to the filter, the optical flow vision information was used to correct the filter.Compared to the general extend Kalman filter, which consists of states prediction step and update step, the ESKF considers the error states induced by the reset process, producing more precise results by reducing the long-term error drift in odometry system.

System State and Propagation
The estimator vector was the 16 × 1 vector, and all the symbol definition can be found in Sec. 2.
E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 1 8 ; 3 2 6 ; 3 7 7 The system kinematic equation that describes the time evolution of the state was similar to Hesch's model, 21 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 x of a quantity x for aircraft position, velocity, and IMU biases as δx ¼ x − x.And it is exceptive for the quaternion error, which was defined by the 3 × 1 angleerror vector δθ, as δq ≜ ½ 1 2 δθ T 1 T .Then, the linearized first-order approximation of continuous-time error-state kinematics 14,20,24 In practical terms, the ESKF is realized in a time-discrete manner, with the update time interval Δ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 E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 2 1 ; 6 3 ; 5 8 5 where Φ k was the transition matrix and G k denoted the perturbation matrix of noise vector n k .In the process of integration and discretization, based on Joan Sola's results, 20 the following auxiliary series was introduced: ; t e m p : i n t r a l i n k -; e 0 2 2 ; 6 3 ; 4 9 1 Γ T n ðωÞ ¼ by assigning the value of n with f0;1; 2;3g, we obtained E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 2 3 ; 3 2 6 ; 7 4 1 Here, Γ T 0 means the incremental rotation matrix if rotating an arbitrary coordinate frame with a rotational rate of −ω for timespan Δt according to the Rodrigues rotation formula; therefore, the discrete linearized error dynamic transition matrix Φ k as a function of above series 24 (for readability ΓT n ¼ Γ T n ½ω m ðkÞ − bω ðkÞ) can be written as in matrix E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 2 4 ; 6 3 ; 4 3 8 ; E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 2 5 ; : The discrete process noise covariance matrix Q k was calculated as E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 2 6 ; 6 3 ; : Thus, the error covariance propagation resulted as E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 2 7 ; 6 3 ; 1 0 4

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 ω ¼ ω m ðtÞ − bω ðtÞ.Substituting the coefficient matrices associating with μ j , ν j , and f in Eq. ( 16), and it would be clearer with notation listed as E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 2 8 ; 6 3 ; 7 1 9 E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 2 9 ; 6 3 ; 5 8 4 Furthermore, by subtracting measured angular rate item and redefining the revised optical flow measurement _ ζj as a new measurement of the filter, the right side was expressed only using the state vector x; therefore, the following notation remains as E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 3 0 ; 6 3 ; 4 8 5 So that the measurement residual, also called innovation, was given as E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 3 1 ; 6 3 ; 3 9 3 y j where the Jacobian measurement was E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 3 2 ; 6 3 ; 3 4 2 E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 3 3 ; 6 3 ; 2 6 0 E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 3 4 ; E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 3 5 ; 6 3 ; 1 8 4 ; t e m p : i n t r a l i n k -; e 0 3 6 ; 6 3 ; 1 1 2 H j δb ω ;k ¼ −B P j : Note that the residual noise η j k was involved with the optical flow algorithm error and gyroscope drift noise, i.e., η j k ¼ ξ j k − B P j • n ω;k , hence, the covariance matrix of the revised optical flow measurement was the sum of the covariance of optical flow algorithm R ξ j and the gyroscope covariance Q ω , which was given as E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 3 7 ; 3 2 6 ; 7 1 8 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, where y k , H k , and η k were composed of the block elements y j k , H j k , and η j k , such that the measurement covariance matrix could be stacked together diagonally as E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 3 8 ; 3 2 6 ; 6 1 5 Then, the a priori state estimate would be updated with current measurements according to the following steps: (1) calculate the Kalman gain as E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 3 9 ; 3 2 6 ; 4 3 2 and (2) calculate the observed error state correction as E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 4 0 ; 3 2 6 ; 3 8 9 with these quantities, the nominal states and error covariance get updated as E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 4 1 ; 3 2 6 ; 3 3 0

Error-State Kalman Filter Reset
For the ESKF reset process, we considered the expression among the new error δx þ k , the old error δx − k , and the observed error correction Δx k , as the true value was constant, yielding Considering that the observed error has been injected into the updated nominal state, i.e., xþ , where the reset operation was to ensure the error would reset δx←0, meanwhile, the covariance of error needed to be modified as well, 14 thus leading to E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 4 2 ; 3 2 6 ; 1 4 9 where G represented the Jacobian matrix according to Joan Sola's proof 15 where errors caused by linearization and discretization are combined with the noise items n k and η k .With the purpose of revealing the theoretical limitation of the observer, a tool for the analysis of the observability properties-local observability matrix MðxÞ was adopted, 25 which was defined by the error dynamic transition matrix Φ k and measurement Jacobian H k , i.e., E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 4 5 ; 6 3 ; 5 3 4 MðxÞ ¼ . .
Here Δt k ¼ t k − t 1 , and the computing method for obtaining analogous expressions of the subblock elements Φ mn can be found in Joel Hesch's work. 26Furthermore, using these expressions of Φ mn , we obtained the k'th block row M k of MðxÞ, for k ≥ 2 E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 4 8 ; 6 3 ;  The proof of the Theorem I is detailed in Sec. 9.
Nullspace N reveals that global rotations about the gravity vector is not observable, and global horizontal position is unobservable while height is fully observable.The remaining states, i.e., the velocity, pitch, and roll angle as well as IMU biases are also observable.The fact that the rotation around the gravity vector is unobservable is due to the missing of an absolute inertial yaw orientation, namely the yaw angle in the inertial frame is unobservable, while the absolute roll and pitch angle of aircraft w.r.t the global inertial frame is made observable by the gravity vector measurement.Similarly, without a global reference, the position in the direction of x and y is not observable, but what is different from previous findings, the altitude is observable because the assumption that optical flow features used for filter updates lie in a level plane and the plane contains the origin of the inertial frame.Only use the feature-plane assumption, according to Eq. ( 6), the feature depth is explicitly determined by absolute altitude and aircraft attitude, which becomes part of the components in the optical flow measurement equation and it, therefore, can get corrected when the new measurement update is coming.

Numerical Simulation
To numerically investigate the previous analysis, the proposed navigation scheme was implemented in a simulation.As shown in Fig. 4, there were 100 features randomly placed in a ½−350; 350 m × ½−350; 350 m horizontal plane around the origin.For generating the ground truth and sensor readings, we simulated a flight with a piecewise continuous trajectory, beginning with constant velocity at 20 m∕s straight line motion lasting for 4 s, following by banking turn with a maximal angle of bank of 30 deg, and then a spiral motion with a pitch angle of up to 9 deg at the altitude between −200 and −300 m, ending with a 50 s of circling.A full list of parameters is shown in Table 1, in which most of those quantities were referred in the commonly used IMU datasheet, e.g., MPU6050 or ADIS16448.
The choices of the initial state values are given in Table 1, which is showing that the filter had fault tolerance to the initial state offset to an extent for convergence.In addition, the initial filter error covariance matrix was set as P 0 ¼ diagð½ 2500I 3 100I 3 0.25I 3 0.01I 3 7.6 × 10 −5 I 3 Þ.Even though using a single optical flow measurement ensured the observability, in a practical trial through proper setting of FOV and initial feature quantity, we would have at least seven features to be extracted every time step to obtain better convergence.After running 100 Monte Carlo simulations, we computed the Monte Carlo standard deviation, also known as the root mean squared error, denoted by the sample error covariance P Monte j through collecting N simulation results as E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 5 0 ; 3 2 6 ; 6 5 3 Here, at the j'th time step, x j was the true state, and xij was the estimated value in the i'th simulation.The Monte Carlo standard deviation and filter standard deviation are the square root of the elements in the main diagonal of P Monte j and ESKF estimation error covariance, respectively.
In Figs.5-9, it is shown the results of the proposed filter tracking ground truth for the position, attitude, velocity, and IMU bias.The shown results outline that most of the state estimations converge to ground truth occurred within 20 s, except the unobservable states, positions along x and y axes and the yaw angle, whose Monte Carlo standard deviation as well as the filter standard deviation at the final time maintained around values at the initial time.The unobservable states could remain with a constant error rather than absolute divergence since they could have proper evolution and IMU biases correction but lacking the initial reference.This correlated well with the observability analysis of horizontal position and yaw in Sec. 4.
Among the observable states, as the observability analysis disregards process noise and initial offset, as the filter standard deviation decreases promptly, the Monte Carlo standard     as vertical velocity, a better agreement between the Monte Carlo sample covariance and the filter error covariance than other states are shown in plots; nevertheless, the estimation of horizontal inertial and body-fixed velocity relied on mutual tight coupling effect with estimated attitude contained with accelerometer bias and unobservable yaw angle.

Experiment
The proposed filter has been tested on an X-star quadcopter which was equipped with Pixhawk 4 and PX4Flow board.system algorithm, which integrated IMU outputs, magnetometer readings, and barometric altitude with GPS data.Note that these extra sensors were only for generating true value and not a part of the proposed navigation system, nor used for estimates.In Fig. 10 the outdoor trajectory during the autonomous controlled waypoint mode flight test is shown.The flight envelope was set with a maximum height at 10 m and maximum speed around 7 m∕s.The quadcopter took off at the origin, as we had little information of the IMU, the estimated states were initialized with zero value as this was the only option.The covariance parameters could be selected based on hardware specifications, which did not require strong tuning as the framework could work well with a large range of parameters.
From results in Figs.11-13, the filter operation was started in the flight at around t ¼ 70 s and at a speed of ∼5 m∕s.The roll and pitch angle exhibit a high-quality tracking accuracy except the growing error occurs when the vehicle was ready to descend for landing at the time after 150 s, as the horizontal velocity drops which was not favorable for the accuracy of optical flow measurement.Due to misalignment of the initial value, the yaw angle always kept a deviation with true value, which might make a major contribution to the estimated error of velocity along x and y axes, especially during t ∈ ½120; 140 s as they deviate from ground truth with a greater oscillation.Under unknown noise of the hardware system or vehicle vibration, the horizontal velocity and horizontal position estimates approached their true value more slowly and less precisely, but vertical velocity, as well as altitude estimate, exhibited more satisfactory accuracy.

Conclusions
This paper has presented an optical flow-based approach to vision-aided inertial navigation.Under the assumption that the environment of the observed feature points can be seen as located on a plane, the feature depth can be obtained by deducing distance expression through the optical flow equation with a geometry-coordinate relationship based on a vehicle-camera-feature system.In order to fill in previous works' gap, an error analysis for optical flow measurement using a block-matching method was provided, which was the prerequisite for adopting and designing an ESKF.With the IMU dead reckoning used for prediction, optical flow, and feature measurement for correction, the estimates of the vehicle's altitude, attitude, velocity, and drift IMU biases can be obtained.The theoretical limitations and feasibility of the filter were investigated by observability analysis, which has shown that only the absolute position along x and y axes along with the yaw angle were unobservable.Finally, the Monte Carlo simulation and hardware involved experiment validated the statement in observability analysis.
The numerical and real experiments have shown that the effectiveness and performance of the proposed filter may suffer from initialization errors and noise.Considering that  the minimal sensor configuration was low-priced and the adopted method required less computational effort, the overall system could be an onboard estimator for providing real-time supplemental state estimate when the UAV would be placed in an unknown GPS-denied environment.
Moreover, with these expressions, and based on Eqs. ( 33)-(36), the k'th block row M k of MðxÞ was calculated, for k ≥ 2 E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 5 2 ; 6 3 ; 5 E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 5 3 ; 6 3 ; 4 5 0 E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 5 4 ; 6 3 ; 3 8 3 E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 5 5 ; 6 3 ; 2 1 6 Proof: To verify the result, we just directly multiplied each block row of MðxÞ with N. When E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 5 7 ; 3 2 6 ; 5 3 0 for E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 5 8 ; 3 2 6 ; 4 6 8 while E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 6 0 ; 3 2 6 ; 3 1 0 and E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 6 1 ; 3 2 6 ; 2 3 4 , and bg×cg ¼ 0 2×1 , it yielded H 1 N ¼ 0 2×3 , M k N ¼ 0 2×3 , so that we had MðxÞN ¼ 0.

E
Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 0 8 ; 6 3 ; 3 7 5 dðX k Þ ¼ arg min −w≤dx;dy≤w SSDðX k ; dx; dyÞ; (8) where I O ð•Þ and I C ð•Þ are both image intensity functions.Thus, the two-dimensional (2-D) optical flow _ ζ can be obtained, i.e., _ ζ ¼ dðX k Þ Given the b × b size block of pixels, the problem aimed at the estimation of ζ by a patch of pixel samples fI O ðp k Þg n¼b×b k¼1 .According to Eq. (9), I O ðp k Þ ∼ NfI C ½ωðp k ; ζÞ; σ 2 g, thus the corresponding probability density function is provided as E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 1 0 ; 3 2 6 ; 3 7 0

)
in which, n log C only contains items irrelevant to ζ.The maximum likelihood estimator ζ maximizes the loglikelihood, i.e., ζ ¼ arg max ζ log LðζÞ, which is equivalent to finding the optimal displacement value that minimizes the least square function according to Eqs. (7) and (8).Therefore, we obtained ζ ¼ arg min −w≤dx;dy≤w SSDðX k ; dx; dyÞ.

)
and both Φ mn and M k are presented in detail in Sec. 8.It is noted that M k4 and M k5 , as containing the nonzero integrals are time-varying matrices and have the linearly independent columns.So that we can only consider the remaining block elements of M k to form the basis of the right nullspace of MðxÞ, which is stated as follows:Theorem I: The right nullspace N of the local observability matrix MðxÞ for the linearized model is spanned by three unobservable directions: E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0

Fig. 4
Fig.4IMU-camera trajectory and feature used in the simulation.

Fig. 5
Fig. 5 Position estimation and corresponding Monte Carlo and filter standard deviation σ hull with respect to the ground truth.

Field
Vol. 58(8)  deviation seems to converge to zero slowly.Comparing with curves of the gyroscope bias, the uncertainty of accelerometer bias presented a much slower convergence, which was induced by the gyroscope bias being explicitly included in the measurement equation and therefore directly corrected while the accelerometer bias only relied on states propagation and obtained the effect via coupling with other states.For the estimation of roll and pitch angle, height as well

Fig. 7
Fig. 7 Body-fixed velocity estimation and corresponding Monte Carlo and filter standard deviation σ hull with respect to the ground truth.

Fig. 6
Fig. 6 Attitude estimation and corresponding Monte Carlo and filter standard deviation σ hull with respect to the ground truth.
The optical flow camera consisted of a 752 × 480 MT9V034 image CMOS sensor with global shutter and 16 mm M12 lens, 24 × 24 μm, whose output to optical flow data was 4 × 4 binned image at 10 Hz.The camera module could be considered as a pinhole camera with the mounting method for pointing the camera straight down.The employed IMU was a dual redundancy solution with BMI055 and ICM-20689, providing inertial measurements at 125 Hz.Ground truth was collected by the conventional INS based on extended Kalman filter and attitude and heading reference

Fig. 9
Fig. 9 IMU bias estimation and corresponding Monte Carlo and filter standard deviation σ hull with respect to the ground truth: (a) the accelerometer bias and (b) the gyro bias.

Fig. 8
Fig. 8 Inertial velocity estimation and corresponding Monte Carlo and filter standard deviation σ hull with respect to the ground truth.

Fig. 13
Fig. 13 Inertial velocity in the test flight.

Table 1
Simulation parameters and initial conditions of the filter ["rand (3, 1)" here represents a 3 × 1 random vector obeying standard normal distribution].
E Q -T A R G E T ; t e m p : i n t r a l i n k -; e 0 5 6 ; 3 2 6 ; 7 5 2M k5 ¼ H δr;k Φ 15 þ H δv;k Φ 25 þ H δθ;k Φ 35 þ H δb ω ;k Deng et al.: Measurement model and observability analysis for optical...Jia Deng received her BS degree at Beihang University, Beijing, in 2010, and she is currently pursuing her PhD at Beihang University, Beijing, China.She mainly studies navigation, control theory, and corresponding application and recently has focused on the data fusion problems and cooperative formation flight control system.Sentang Wu received his PhD in dynamics, ballistics, and aircraft motion control systems from Ukraine National Aviation University in 1992.He mainly studies the theory and application of nonlinear stochastic systems, computer information processing, and control and aircraft cooperative control.He is currently a professor of automation science and electrical engineering and a doctoral tutor at Beihang University.Hongbo Zhao received her BS degree at Beihang University, Beijing, and she is currently pursuing her PhD at Beihang University, Beijing, China.She mainly studies the control theory and application, distributed state estimation of stochastic systems, which focus on multiagent systems, and cooperative formation flight control systems with uncertainties.Da Cai received his BS degree in control science and engineering from Beihang University, Beijing, China, in 2012 and also received his PhD in control science and engineering in 2018.His research interests include multirobot networks, distributed algorithms, and aircraft cooperative control.Deng et al.: Measurement model and observability analysis for optical. . .