Vision-Based State Estimation

The Vision-Based State Estimators (VBSE) is a class of estimation filter based on the algorithms presented in Section 2.4 and formulated to use real-time optical information in addition to more conventional measurements. For planetary landing missions, the vast majority of the papers use EKF [10, 76] while some others base their design on SPKF [131]. All designs use the optical measurements to improve inertial-measurement only navigation. In addition, the filter algorithms are often formulated to solve a joint estimation problem. More precisely, the states of the filter describe not only the attitude and position of the spacecraft, but also the inertial measurement biases as it is shown in the following state vector:

𝒙 = [𝒒𝐵𝑃 𝒃𝜔𝐵 𝒗𝑆𝑐𝑃 𝒑𝑆𝑐𝑝 𝒃𝑎𝐵] 𝑇

(2.66)

where 𝒒_𝐵𝑃 is the quaternion that characterizes the orientation of the spacecraft body frame relatively to the planet frame, 𝒗𝑆𝑐𝑃 and 𝒑𝑆𝑐𝑝 are respectively the velocity and the position of the

spacecraft in the planet frame, 𝒃𝜔𝐵 and 𝒃𝑎𝐵 are respectively the biases of the gyroscope and of the

accelerometer (slow varying measurement offset which affect all inertial measurement instruments). The time update of the filter is achieved by propagating the spacecraft attitude quaternion and velocity using the bias-corrected gyroscope and accelerometer measurements.

As explained in Chapter 1, there are two categories of optical measurements: absolute and relative. From the filter point of view, there are two main differences between absolute and relative features. First, in addition to their image coordinates, the planet-surface position of the absolute features is available. This information is uncorrelated with the states of the spacecraft and brings the observability of the position states. Second, the image coordinates of the relative features are available for at least two images taken at different time instants. The most widespread measurement update strategies using these two types of measurements are summarized in the following paragraphs.

2.5.1.

Absolute Optical Measurement Update

The most widespread strategies to fuse feature information, with a priori knowledge of their surface position, consist in using the camera pinhole projection [132]. The pinhole projection is illustrated in the figure below.

Figure 2.33: Camera Pin-Hole Projection

This projection describes the relation between the normalized image coordinates 𝒖𝑖 of the feature 𝑖

and its three-dimensional position on the surface using the position of the projection center and the orientation of the camera. Assuming that the camera frame is centered on the projection center of the camera and that the boresight of the camera is aligned with the z-axis of its frame, the pin-hole projection model can be simply seen as the 𝑥 and 𝑦 components of the vector between the feature position and the position of the camera projection center expressed in the camera frame divided by the 𝑧 component of the same vector. The z component of this vector is often referred to as the depth of the feature. The relation between the normalized image coordinates of the feature and its coordinates in pixel is obtained from the intrinsic parameters (focal distance and coordinates of the

Surface position of the feature i Projection center of the camera ui Image plane of the camera

projection center) as well as the lens distortion coefficients. Please refer to Chapter 4 for more details about this. The vector 𝒗𝑖𝐶 = [𝒖𝑖, 1]𝑇, called homogenous coordinates of the feature, corresponds to

the position of the feature projected onto the image plane with respect to the camera projection center expressed in the camera frame. This vector can be used to get the direction of the feature in the camera frame. The product between the homogenous coordinates of the feature and its depth gives its three-dimensional position on the surface expressed in the camera frame.

As a function of the spacecraft state variables, this model is described by the following equation:

𝒖 ̃𝑖=

[1 0 0_{0 1 0}] 𝒑𝐹𝑒𝑎𝐶 _𝑖

[0 0 1]𝒑𝐹𝑒𝑎𝐶 _𝑖 + 𝜼𝐴𝑏𝑠,𝑖

(2.67)

where 𝒖̃_𝑖 is the measurement of the feature 𝑖 in normalized-image coordinates,𝜼_{𝐴𝑏𝑠,𝑖} is the feature measurement noise, 𝒑_𝐹𝑒𝑎

𝑖

𝐶 _{is the feature position expressed in the camera frame defined as:}

𝒑𝐹𝑒𝑎𝐶 _𝑖= 𝑪(𝒒𝐶𝐵)[𝑪(𝒒𝐵𝑃)𝒍𝑖𝑃− 𝒑𝐶𝑎𝑚𝐵 ] (2.68)

𝒒𝐶𝐵 is the attitude quaternion of the camera with respect to the spacecraft body frame and

𝒍_𝑖𝑃_{= 𝒑} 𝐹𝑒𝑎𝑖

𝑃 _{− 𝒑}

𝑆𝑐

𝑃 _{is the component of the vector defined between the three-dimensional position of}

the feature and the vehicle position expressed in the planet frame. The absolute optical measurement model often includes the surface position error of the feature also known as map-tie error as well as the misalignment of the camera (due to vehicle vibration or elastoplastic deformation) [8].

A second technique consists in computing the pose of the spacecraft from multiple feature observations using a state-of-the-art optimization algorithm. At least two and three observations are respectively required to compute the position and the 6-DOF pose of the vehicle. This computed spacecraft pose, not correlated with the estimated states of the spacecraft, is then used in the measurement update of the filter.

2.5.2.

Relative Optical Measurement Update

In the literature, the strategies used to fuse the relative optical measurement can be organized into two categories.

The first solution is to augment the state vector of the estimator with the line of sight to each tracked feature [133]. Then, the goal is to estimate these quantities from the measurements of their position

in normalized image coordinates through a sequence of images. The feature states are propagated by the following equation:

𝒍̇𝑖𝑃= −𝒗𝑆𝑐𝑃 (2.69)

where 𝒗𝑆𝑐𝑃 is the estimated spacecraft velocity in the planet frame. The measurement model of Eq.

(2.67) is used with the states 𝒍𝑖𝑃 for the update phase. The advantages of this technique are: the

estimated position of the features can be used for Hazard Detection (HD), the measurement update of the filter is straight forward, each measurement can be treated sequentially, the measurement model is simple and is the same as the one used to process the absolute optical measurements. However, the size of the state vector increases with the number of tracked features. The estimation algorithm must manage the lost and the appearance of tracked features (track that disappears must be replaced by new track which needs to be initialized using empirical and complex methods). The measurement delays introduced by the image processing algorithm are not intrinsically managed. This means that techniques similar to that introduced in Section 2.4.9 must be implemented which require additional computation power.

The second method, proposed by [8, 43, 134], reduces the computational power associated with the state augmentation related to the estimation of the position of each tracked feature. Every time an image is taken from the camera, the state vector is augmented with a copy of the current spacecraft poses (position and attitude). Only a fixed number of past spacecraft poses are kept. The new one replaces the older. These augmented states are fixed in time and only their cross covariance with the current spacecraft states changes. This state augmentation approach is very similar to the one used for delay recovery, but instead of keeping only the states corresponding to the last measurement, a history of past vehicle poses corresponding to several past relative optical measurements is maintained in the state vector. When the track of a given feature is lost, the past spacecraft poses at which this feature has been seen and the correspoding image coordinates of the feature are used to compute its position in the planet frame. The surface position of a given feature can be estimated from at least two observations of this feature at two different known locations. However, it is preferable to have more observations in order to get a more accurate result. This estimation is done by a specialized optimization algorithm outside of the navigation filter. The estimated surface position of the feature is then used to update all past time-fixed spacecraft poses with the measurement model of Eq. (2.67). However, the estimated feature position is strongly correlated with the states of the filter. The measurement residue is then decorrelated using a technique

involving the left null space of the Jacobian of the measurement model relative to 𝒑𝐹𝑒𝑎𝑖

𝑃 _{. The}

complete derivation of this approach is presented in Chapter 8. One major advantage of this approach is that the image processing delay is intrinsically managed, because updates always apply on past states of the spacecraft (augmented part of the state vector). In addition, the size of the state vector is independent from the number of tracked features. The measurement model is the same as the one used for absolute optical measurements. However, the need to compute a null space of a matrix at each update to decorrelate the measurements from the spacecraft states requires a high computational power. The update can occur only when a track is lost or when the track is long enough to compute the surface position of the feature. In addition, the update cannot be done sequentially for each measurement, but in batch for all the measurements of a given feature track. Finally, it requires the use of a computationally complex optimization algorithm to compute the feature position from past spacecraft pose and feature track measurements.