Abstract
An integrated navigation scheme based on multiple optical flow sensors and a strapdown inertial navigation system (SINS) are presented, instead of the global position system (GPS) aided. Multiple optical flow sensors are mounted on a micro air vehicle (MAV) at different positions with different viewing directions for detecting optical flow around the MAV. A fault-tolerant decentralized extended Kalman filter (EKF) is performed for estimating navigation errors by fusing the inertial and optical flow measurements, which can prevent the estimation divergence caused by the failure of the optical flow sensor. Then, the estimation of navigation error is inputted into the SINS settlement process for correcting the SINS measurements. The results verify that the navigation errors of SINS can be effectively reduced (even more than 9/10). Moreover, although the sensor is in a state of failure for 400 seconds, the fault-tolerant integrated navigation system can still work properly without divergence.
1. Introduction
In recent years, unmanned air vehicles have been widely used in military and civil fields. State-of-the-art navigation schemes for unmanned aerial vehicles (UAVs) typically rely on the high-quality global position system (GPS). However, the GPS signal is not always available (indoor places, for example), so the overreliance on GPS is becoming a prominent insufficiency of UAVs [1]. As a result, the increasing interest in navigation for a GPS-denied environment has heightened the need for novel GPS-free integrated navigation schemes. Inspired by flying insects (e.g., honeybees) that have the ability to fly with great agility, scientists hope that micro air vehicles (MAVs) could fly like the insects without the aid of GPS signals [2]. Experiments show that one of the navigation information sources is measuring the moving velocity of the image in the world appearing in the eye when honeybees fly to their destination [3]. Researchers think that what the honeybees are mainly relying on is the apparent motion of the objects in their field of view, namely, optical flow, which contains egomotion information relative to the environment of the flying insects [4].
On the other hand, the requirement for more self-reliant (autonomous) navigation systems and the need for MAV with a greater understanding of their environment are becoming more and more urgent. It is obvious that insects like honeybees, even with their small brains and limited intelligence, have the accurately autonomous navigation ability described above. Accordingly, bioinspired optical flow sensors are developed with the advantage of smaller size, lighter weight, low power requirement, higher frequency, and lower cost compared to other equipments such as lidar, radar, and magnetometer [5].
Chahl [6] has also pointed out that the largest development opportunities may exist in small and micro UAV domains as a result of the novelty of aerospace engineering on a small scale. Therefore, autonomous navigation or pose estimation using optical flow sensors is valuable for MAVs. This has motivated many researches into optical-flow-based MAV navigation systems and algorithms.
Ruffier and Franceschini [7] have developed an optical flow regulation loop-based MAV which is able to take off, cruise, and land automatically. The MAV can also react appropriately to wind disturbances. This MAV can keep the downward optical flow at a constant value. Furthermore, the two-degree-of-freedom (DOF) tethered MAV was shown to land safely on a platform set into motion along two directions, vertical and horizontal, without ground height or groundspeed information [8]. The results show that their MAV works very well. However, the MAV has only two DOF, which is a little far from the 6 DOF practical MAV.
As stated earlier, the optical flow contains the egomotion information of honeybees or MAVs. Consequently, it is a reasonable navigation frame that combines the optical flow and the inertial navigation system (INS) measurement information in order to improve the navigation precision.
Sabo et al. [9] performed an approach for bioinspired navigation using optical flow. Hallway navigation results show that the roll, pitch, and thrust can be tuned and controlled as expected. The deficiency is that the optical flow was calculated off-board using a computer vision algorithm, rather than taking advantage of optical flow sensors with small computation and short response time.
Zhang et al. [10] fused strapdown inertial navigation system (SINS) information with optical flow through a Kalman filter for correcting the SINS attitude when it is diverged. Simulation results show that the estimated vehicle attitude is of good performance with smaller error. Nevertheless, the placement scheme of optical flow sensors is not shown in this paper. In addition, it is considered that the pitch angle is sinusoidal, yet the roll angle and yaw angle are all zero, which is a little far from the practical situation.
In order to overcome the drift of INS, Shen et al. [11] designed an INS/optical flow/magnetometer integrated navigation scheme for a GPS-denied environment. The gyro, accelerator, and magnetometer information is properly fused for estimating the attitude of the MAV. The results show that the proposed scheme can effectively reduce the errors of navigation parameters and improve navigation precision. However, too many devices are involved in navigation, and the corresponding functions can be realized only by fusing optical flow and inertial information.
Lyu et al. [12] designed a fault-tolerant filter based on optical flow and inertial measurements. The velocity estimation accuracy is greatly improved. In want of perfection, the utilization of optical flow information needs to be further improved, not only for estimating velocity.
In this paper, a multiple optical flow sensor/SINS on-board integrated navigation scheme for a GPS-denied environment is proposed. Multioptical flow sensors are fixed on the given MAV, referencing an optimal placement scheme in order to make the optical flow sensors as sensitive as possible to the motion state change of a MAV. Considering that the MAV may make a big maneuverer so that some optical flow sensors may face the sky, which causes the failure of optical flow sensors, an adaptive decentralized filter with fault detection and isolation ability is therefore developed to conquer this problem. 6 DOF flight simulations are carried out for validating the effectiveness of the navigation algorithm.
The rest of this paper is organized as follows. Section 2 introduces the mathematical model of optical flow sensors. The optical flow sensor/SINS integrated navigation algorithm based on the extended Kalman filter (EKF) is described in Section 3. Section 4 gives the optimal placement scheme of multiple optical follow sensors fixed on a MAV. Section 5 presents an application of the centralized EKF. A fault-tolerant integrated navigation scheme is proposed in Section 6. In the last section, the author arrives at the conclusion that the fault-tolerant optical flow sensor/SINS integrated navigation algorithm is effective.
2. Optical Flow Sensor Model
The magnitude of optical flow is defined as the angular rate at which a point on an object point moves relative to an optical flow sensor. The perspective projection model of the optical flow sensor [10, 11] is illustrated in Figure 1.
2.1. Optical Flow Definition
Let be a point on the ground in the three-dimensional optical flow sensor reference coordinate frame . Let the optical axis of the optical flow sensor be the -axis of the optical flow sensor coordinate system. is the focal length and the origin is the centre of projection. and are the pixel numbers of the optical flow sensor image plane along the and directions, respectively. The projection of point on the image plane is given by
Point has the following relative motion to the optical flow sensor under the sensor coordinate system : where is the angular velocity and the translational component of the motion relative to the optical flow sensor.
Let (2) be expanded in three dimensions; we get the following equation:
Taking the derivative of both sides of (1) with respect to time, the relation between the velocity of in the optical flow sensor coordinate system and the velocity or the optical flow of in the image plane [13] is obtained.
Let (4) be expressed in and components, we get
Substituting (3) into (5) and (6), the following equations are achieved: where and are the optical flow components on directions and , which can be calculated by a matching algorithm, such as the sum of absolute differences (SAD).
When an optical flow sensor is mounted on a MAV flying in a three-dimensional space, the optical flow is a function of the MAV motion state (translation states and rotation states ) and the distance from the optical flow sensor to the object point, as shown in Figure 2.
In order to make the optical flow sensor work properly, the object plane should have adequate texture, and it should be sufficiently illuminated for the sensor to distinguish and track features. The optical flow sensor measures only two quantities which are the and components of the mean optical flow within its field of view. The local optical flow vector in the viewing direction (i.e., the direction of the optical axis) experienced by the optical flow sensor at velocity and rotational rate is [14] where is the distance between the object point and the optical flow sensor in direction .
For a simple example, Figure 2 depicts an optical flow sensor moving relative to its object plane at a right velocity , a height above the ground , and a yaw rate . The angle between the viewing direction (the direction of the optical axis) of the optical flow sensor and the plumb line is . The optical flow detected by the optical flow sensor in the plane formed by the optical axis and the velocity vector is [5]
The physical unit of in (8) and (9) is “”.
2.2. Reference Coordinate Systems
In order to describe the 6 DOF motions of a blended wing body [15] MAV with multiple optical flow sensors in three-dimensional space, three reference coordinate systems as can be seen in Figure 3 [16] are defined. (i)Navigation coordinate system (): a local ENU (east-north-up) frame fixed to a world is selected as the navigation frame. Absolute motion states of the MAV are defined in this frame. If the rotation of the Earth is neglected, the ENU reference coordinate system can be considered as an inertial coordinate system.(ii)MAV body coordinate system (): a coordinate system fixed on the MAV body. Its origin is the centre of gravity of the MAV body. The -axis is parallel to the longitudinal axis of the MAV body and points forward. The -axis points upward.(iii)Optical flow sensor coordinate system (): a coordinate system fixed to a optical flow sensor. Its origin is the focus of the optical flow sensor. The -axis is the optical axis of the optical flow sensor. The -axis and -axis are parallel to the optical flow sensor’s -axis and -axis, respectively.
The transformation matrix from the MAV body coordinate system to the optical flow sensor coordinate system is where and are the mounting angles of the optical flow sensor fixed on the MAV. Namely, is the angle around the axis for rotating to , and then is the angle around the axis.
Thus, the transformation matrix from the local coordinate system to the optical flow sensor coordinate system is where is the transformation matrix from the local coordinate system to the MAV body coordinate system .
2.3. Expression of Optical Flow in Three-Dimensional Space
As the measurements of optical flow sensors are based on the optical flow sensor coordinate system , the motion states of the MAV in the inertial coordinate system need to be projected onto the optical flow sensor coordinate system .
Therefore, based on the coordinate systems defined in the above section, the optical flow experienced by an optical flow sensor in Figure 3 is where and are the and component magnitude of defined in (8), which are the measurements of the optical flow sensor. and are the velocity and rotation rate of the optical flow sensor in the inertial frame , respectively. Subscripts and denote the and components in the optical flow sensor coordinate system , respectively. is the distance along from the lens centre of the optical flow sensor to the ground.
Next, we need to find the relationship between the optical flow measurements in the optical flow sensor coordinate system and the motion state of the aircraft in the inertial coordinate system .
First, the velocity vector of the optical flow sensor in the inertial frame can be expressed as
The projection of in the coordinate system is
As mentioned earlier, the navigation coordinate system can be considered as an inertial coordinate system, when the rotation of the Earth is neglected. Then, it should be noted that is indeed the rotational angular velocity of the MAV in the inertial reference system, which can be directly measured by SINS.
Second, the direction vector of is defined as , which means that . Thus, the projection of to is
Therefore, the cosine of the angle between and is , and where is the corresponding element of .
The distance along the axis from the origin of the optical flow sensor coordinate system (i.e., the focus of the optical flow sensor) to the ground is
Third, the rotational angular velocity of the optical flow sensor in the optical flow sensor coordinate system is
Finally, substituting (14), (17), and (18) into (12), the measurements of an optical flow sensor are
The measurement noise of in (19) is modelled by a white noise vector with zero mean in the following simulation, whose variance matrix is , where .
3. Information Fusion Using EKF
In order to improve the precision of SINS, the navigation errors are taken as the state vector in this study instead of navigation parameters. This is because the navigation errors are much smaller than the value of the navigation parameters, and the change of navigation errors is slow. Therefore, the state transmission of navigation errors can be expressed accurately with linear equations, and the estimation precision of the state can be easily guaranteed.
The state vector is taken as [17] where the subscripts , , and stand for the components in the corresponding axis of the east-north-up local coordinate system ; , , and stand for the components in the corresponding axis of the MAV body coordinate system ; denote the position errors of SINS; denote the velocity errors of SINS; denote the attitude errors of SINS; denote the random bias errors of the gyro in SINS; denote the random walk process errors of the gyro in SINS; and denote the random walk process errors of the accelerometer in SINS.
Then, the state equation of the EKF can be represented as where is a 18 × 18 SINS error matrix [17] and is a 18 × 9 system noise allocation matrix given by
is the system noise vector with zero mean, that is, where , , an are the gyro random white noises; , , and are the first-order Markov-driven white noises of gyro; and , , and are the first-order Markov-driven white noises of the accelerometer.
The covariance matrix of system noise vector is where , , and .
The estimated navigation errors are expected to be used for correcting the navigation parameters of SINS. There are two methods to correct the navigation parameters [18]: one is to directly correct the outputs of SINS by taking the estimated value of the navigation error as the feedback, which is called the output correction method; the other way is to feed the estimation value of the navigation error into the SINS settlement process, which is called the feedback correction method. The output correction method only changes the accuracy of the output, rather than the error state inside the inertial navigation system. In this way, the errors will accumulate gradually over time, making the difference between the ideally mathematical model and the actual navigation system larger and larger, and the accuracy of the integrated navigation system becomes poor. Therefore, the feedback correction method is adopted in this paper in order to increase the accuracy of SINS.
Finally, the structure of the optical flow sensor/SINS integrated navigation scheme based on EKF is shown in Figure 4. It is mainly composed of the following four modules. (1) SINS. The microelectromechanical system- (MEMS-) based SINS outputs the attitudes, position, and velocity measurements with drifts and high noises. As time goes by, the navigation errors of SINS will gradually increase, so some other navigation devices should take the role of correcting the SINS errors, such as the optical flow sensors introduced before. (2) A Group of Multiple Optical Flow Sensors. Multiple optical flow sensors are mounted on a MAV at different positions with different viewing directions. As a result, the optical flow information in different directions around the MAV can be adequately detected. (3) Prediction of the Optical Flow Using the SINS Measurements. Optical flow can be predicted by using (19) with the SINS measurements as the input. is the difference of the optical flow sensor measurements and the predicted ideal optical flow. The optical flow difference is taken as the measurements of EKF. So the measurement equation of EKF is where is the measurement matrix [17], and is the measurement noise vector with zero mean, whose covariance matrix is where . (4) Extended Kalman Filter (EKF). A centralized extended Kalman filter is first performed in this study for fusion of inertial and optical flow information. The state equation of EKF is expressed by (21), and the measurement equation is expressed by (25). The navigation errors of SINS can be corrected by using the navigation error estimation feedback of the EKF.
4. Sensor Placement Optimization
As mentioned in the previous section, multiple optical flow sensors are fixed on the MAV. So the placement scheme of optical flow sensors should be taken into account, which consists of the following three issues: (a)The positions where the optical flow sensors are mounted(b)The orientations which the optical flow sensors are facing(c)The number of optical flow sensors
4.1. Mounting Position of Optical Flow Sensors
As illustrated in Figure 3, different means that the optical flow sensors are fixed on different positions of the MAV. Several magnitudes of are used for investigating the influence of optical flow sensor positions on the navigation performance. The placement position and angle of the optical flow sensors in the MAV body frame are shown in Tables 1–4. The sensor mounting angles in each case are identical.
It can be concluded from Figure 5 that the navigation performances of each case are nearly the same, though the mounting positions of each case are very different. In other words, mounting positions of the optical flow sensors have little influence on the performance of the navigation system. On the other hand, if the distances between each optical flow sensor and the gravity centre of the MAV are as long as possible (case 4), the longitudinal velocity navigation error is slightly smaller than the ones in other cases, which means that the navigation performance is slightly better.
4.2. Mounting Orientation of Optical Flow Sensors
We expect that the optical flow sensors can be as sensitive as possible to the motion state changes of MAV. From (15), (16), (17), (18), and (19), it can be found that the magnitude of optical flow is a function of the MAV motion state and the optical flow sensor mounting angles . Thus, where is a function of , , and .
Let represent the optical flow sensitivity, the sensitivity can be defined as where is the motion state of MAV at a certain moment and is the motion state change of MAV. Here, the motion state and its change of a MAV for investigating the influence of mounting angles on the sensor sensitivity are shown in Table 5.
Based on the current state shown in Table 5, the mounting angles and change from 120 to 240 degrees and −60 to 60 degrees, respectively. The relationship between the measuring sensitivity and the mounting angles of optical flow sensors is illustrated in Figure 6. The results indicate that when the optical flow change is the most obvious (in other words, when the sensitivity of the optical flow sensor is the highest), the ranges of mounting angles are
4.3. Number of Optical Flow Sensors
As we know, more optical flow sensors provide more measurement information, and the redundancy of the navigation system can also be enhanced, making the system more reliable. However, too much information input from optical flow sensors will increase the computational burden of the on-board computer on MAV. As a result, the real-time performance of the integrated navigation algorithm may fall. Moreover, too many optical flow sensors will also increase the payload mass of MAV. In contrast, too few optical flow sensors will lead to poor estimating accuracy of the EKF filter.
Different quantities of optical flow sensors are fixed on the MAV for investigating the influence on the navigation performance. The first one, the first two, the first three, and all optical flow sensors in Table 4 are installed on the MAV in turn.
Taking the longitudinal velocity error as an example, the results shown in Figure 7 indicate that, with the aid of optical flow information, the navigation error is obviously smaller. If the number of optical flow sensors is more than one, the navigation error will be further reduced, about 1/10 of the navigation error from the one optical flow sensor-aided inertial navigation system.
Moreover, if the quantity of optical flow sensors is greater than two, the navigation performance of the integrated navigation system is the same as the inertial navigation system aided with two optical flow sensors. Considering the navigation accuracy, computational cost, payload mass, and redundancy, three optical flow sensors are recommended to be mounted on the MAV.
In summary, the optimal placement scheme shown in Table 6 is that three optical flow sensors are mounted on the MAV, as far apart as possible from each other and the gravity centre of the MAV. The optimal mounting angles can be referred to Formula (29).
5. Numerical Simulation Using a Centralized EKF
To test the performance of the optical flow sensor/SINS integrated navigation scheme, three optical flow sensors are mounted on the MAV using the optimal placement scheme shown in Table 6. Some other initial parameters for this simulation can be seen in Table 7.
Navigation errors of the optical flow sensor/SINS integrated navigation scheme using a centralized EKF compared with the navigation system using SINS only are displayed in Figure 8. The results indicate that the navigation errors (including position, velocity, and attitude errors) become much smaller after optical flow sensors aided. The correctness and effectiveness of the optical flow aided inertial navigation scheme are verified.
(a)
(b)
(c)
In contrast, the premise that the optical flow sensor can work well is that the detection plane has sufficient texture, and the illumination is sufficient. Hence, if the MAV makes a big maneuver, some optical flow sensor(s) may face the sky, which leads to the failure of optical flow sensors as a result of zero measurement. In this case, the navigation system will diverge, suffering from inputting wrong information.
For example, let the output of the third optical flow sensor in Table 6 be zero from 300 s to 700 s; the longitude error of the integrated navigation system using centralized EKF is illustrated in Figure 9.
As can be seen in Figure 9, the navigation error immediately diverges, when one of the optical flow sensors fails. Accordingly, an integrated navigation scheme with fault detection and isolation ability is proposed in the subsequent sections.
6. Fault-Tolerant Integrated Navigation Scheme
6.1. Decentralized Filter
There are two ways to achieve an optimal integrated navigation system based on the extended Kalman filter technology: one is centralized Kalman filter, and the other is decentralized Kalman filter.
The centralized Kalman filter is the method used in the previous sections, which utilizes only one Kalman filter to process all the information of all the navigation subsystems (SINS and optical flow sensors). The centralized EKF integrates the SINS subsystem with the optical flow sensor subsystems optimally to estimate the navigation error state and then correct the SINS subsystem by using the optimal estimation of the navigation error state. Thus, navigation precision can be improved relative to the navigation system using the SINS only.
The decentralized filter used in this study is a federated filter. By using the principle of “information distribution,” the best compromise can be obtained through adjusting the distribution weights of subsystem information in the navigation system. Therefore, the federal filter used in this project can achieve the following advantages against the centralized filter: (1)The fault-tolerant performance of the decentralized filter is better. When one or more navigation subsystems fail, the fault can be detected and isolated. Afterwards, the remaining normal navigation subsystems can be reassembled or reconfigured quickly to get the required accurate filtering solution.(2)For the centralized filter, the increase in state dimension will bring the so called curse of dimensionality, which makes the computational cost increase dramatically. In addition, reducing the filter dimension will lose the filtering accuracy and even cause filtering divergence. As for the decentralized filter, the synthesis (fusion) algorithms of both local filters and the master filter are simple, with less computation, so as to facilitate the real-time implementation of the navigation algorithm.
Referring to Figure 10, the workflow of the federated filter can be summarized as follows: (1)The initial estimated covariance matrixes of local filters and the master filter are set to () times that of the integrated navigation system; satisfies the principle of information conservation equation, that is, (2)The process noise covariance matrixes of local filters and the master filter are set to times that of the combined system process.(3)Each local filter obtains the local estimation by processing its own measurement information.(4)The local estimation of each local filter and the estimation of the master filter are integrated according to where is the th local state estimation and is the corresponding estimated covariance matrix. The local estimates are not related to each other, namely, . is the global optimal estimate, whose corresponding covariance matrix is .(5)The global optimal estimate is used to reset the state estimation and covariance matrix of local filters and the master filter.
The determination of the information distribution coefficient is crucial to the decentralized filter [19]. In this study, considering the fault tolerance and computational complexity of the decentralized filter, the information distribution coefficient of the master filter is , and the information distribution coefficient of each local filter is (). Therefore, the estimation of the master filter is actually the global estimation, namely,
6.2. Fault Detection and Isolation
In the fault-tolerant integrated navigation system, the validity of the measurement information of each local filter must be determined in real time so as to determine which local state estimations are used to calculate the global state estimation. This requires that a real-time fault detection and isolation algorithm should be performed in the local filtering process. As a result, once a fault is detected, the fault can be isolated. Consequently, by reconstructing the system information, the whole navigation system will not fail due to the failure.
A discrete system model with failure is expressed by where is the measurement of the navigation system. is the system state. is the one-step transformation matrix of the system state. is the system noise matrix. and are mutually independent Gauss white noise sequences with the following constrains: where is the Dirac function. is a random vector, which indicates the degree of a fault. is a piecewise function where is the occurrence moment of the fault.
Here, the residual detection method is used to detect and isolate the faults of the navigation system.
The residual of each local Kalman filter is where the forecast value is
It can be proved that the residual of Kalman filter is a Gauss white noise whose mean is zero when no fault occurs, and its variance is
When a fault occurs, the mean of residual is no longer zero. Therefore, by monitoring the mean of residuals, it is possible to determine whether the navigation system has failed.
The fault detection function is defined as where is subject to distribution with degrees of freedom, namely, where is actually the dimension of measurement . Here, .
Finally, the fault criteria are as follows: where the preset threshold is determined by the false alarm rate . Given the false alarm rate , the distribution can give out the threshold value .
In this study, and , then according to the distribution, the threshold value .
As each local filter is designed for the measurement information from each optical flow sensor, the fault detection and isolation algorithm in the local filter can be used for detecting the invalid measurement information and the corresponding local error state estimation. Using the remaining correct local state estimation, the reliable state estimation of the whole navigation system can be obtained according to the decentralized filter proposed above. The structure of the above proposed fault-tolerant optical flow sensor/SINS integrated navigation scheme is shown in Figure 11.
6.3. Numerical Simulation Using the Fault-Tolerant Integrated Navigation Algorithm
As before, three optical flow sensors are mounted on the MAV using the optimal placement scheme shown in Table 6. We assume that the third optical flow sensor in Table 6 fails (or no apparent texture on its detected plane) during 300 s–700 s with zero outputs, as can be seen in Figure 12. Some other initial parameters are shown in Table 7.
(a)
(b)
As shown in Figure 12, when the optical flow sensor fails, the value of fault detection function sharply increases, much greater than the threshold value .
In the subsequent figures (Figures 13–15), the “fault zone” is the period that the optical flow sensor fails, and the normal working state is restored after 700 s.
(a)
(b)
(c)
(a)
(b)
(c)
(a)
(b)
(c)
The meanings of legends in each figure are as follows: (1)“DF-no fault”: the navigation error of using the decentralized filter for MAV navigation. During the flight of MAV, the optical flow sensors have no fault.(2)“DF-FDI”: the navigation error of using the decentralized filter with fault detection and isolation (FDI) capability for MAV navigation. During 300 s to 700 s, the outputs of the third optical flow sensor in Table 6 are set to zero.(3)“DF-no FDI”: as in the previous case, the outputs of the third optical flow sensor in Table 6 are set to zero during 300 s to 700 s. The difference is that the decentralized filter does not have the FDI capability.(4)“CF-no fault”: the navigation error history of using the centralized filter for MAV navigation. During the flight of MAV, the optical flow sensors have no fault.(5)“CF-fault”: the navigation error of using the centralized EKF for MAV navigation. Of course, the centralized filter does not have the FDI capability. During 300 s to 700 s, the outputs of the third optical flow sensor in Table 6 are set to zero.
For the centralized filter and decentralized filter without FDI capability, the global estimation immediately diverges when the optical flow sensor fails, causing the failure of the whole system, namely, “failure of FDI-free filters,” shown in the following figures. In contrast, the decentralized filter with the FDI capability can detect and isolate the sensor failure in time to make the navigation system work properly, namely, “effectiveness of the FDI filter” as can be seen in the following figures. The results verify the correctness and effectiveness of the designed fault-tolerant optical flow sensor/SINS integrated navigation scheme in the presence of system-level failures.
In addition, as can be seen in the figures, the curves of “DF-no fault” and “CF-no fault” are consistent, and the curves of “DF-no FDI” and “CF-fault” are consistent, which indicates that the centralized filter and decentralized filter are mathematically equivalent.
In order to detect the effect of the sampling period on the performance of simulation, the 1200 s MAV flight simulation using the flight navigation scenarios proposed in this manuscript are performed in desktop environments (Intel® Core™ i7-6700 CPU at 3.40 GHz, Matlab R2015a), under different sampling periods (0.1 s, 0.02 s, 0.01 s, 0.002 s, and 0.001 s). The simulation results are obtained from 50 runs of Monte Carlo.
Three navigation schemes are considered in the simulation: (1)Navigation only based on INS (inertial navigation system), called “INS only” in Table 8(2)Optical flow sensor/INS-based integrated navigation scheme using the centralized EKF, called “centralized EKF” in Table 8(3)Optical flow sensor/INS-based integrated navigation scheme using decentralized EKF with FDI (fault detection and isolation) ability, called “FDI-EKF” in Table 8
Here, two criteria are used for evaluating the simulation performance. One is the computing time and the other is the navigation error. Furthermore, the criterion employed for evaluating the navigation error with respect to the sampling period is the RMSE (root mean square error), over M Monte Carlo runs at a given time , which is defined by where superscript denotes quantities on the run. denotes the truth of one of the MAV motion states at time and for the run of Monte Carlo simulation; the same is but being the estimation of the state.
Taking the RMSE of as an example, as indicated from Figure 16, the shorter the sampling period, the smaller the navigation error. On the other hand, the average computing time spent in 50 Monte Carlo simulations of different navigation algorithms under different sampling periods is shown in Table 8. It can be found that the shorter the sampling period, the longer the computation time used for simulation. When the decentralized EKF with FDI ability is deployed under the 0.001 s sampling period, the computing time (1911.1021 s) has even exceeded the simulation time (1200 s), which means that the navigation algorithm cannot be implemented in real time.
Conversely, as can be seen in Table 8, the larger the sampling period, the shorter the computing time. Actually, increasing the sampling period makes the amount of computation smaller. However, as shown in Figure 16, if the sampling period is too long (e.g., 0.1 s), the filter will diverge.
Accordingly, a series of sampling periods are employed in the simulation in order to determine an appropriate one for the optical flow sensor/INS-based integrated navigation scheme using decentralized EKF with FDI ability (FDI-EKF). As mentioned above, the filter diverges if the sampling period is too long. For the FDI-EKF, after many simulations, it is found that the navigation scheme can work correctly when the sampling period is equal to or less than 0.02 s.
As portrayed in Figure 17, when the sampling period is reduced from 0.02 s to 0.01 s (i.e., reduced by 1/2), the navigation error is greatly reduced. However, although further reduction in the sampling period will result in a slight reduction in the navigation error, Table 8 indicates that the computing time will multiply exponentially, even more than the 1200 s simulation time. Finally, 0.01 s is selected as the sampling period in Table 7 after the trade-off between the navigation accuracy and computing time.
7. Conclusions
In this paper, an optical flow sensor/SINS integrated navigation scheme for MAV is developed for reducing the navigation error of the inertial navigation system through the decentralized EKF with fault detection and isolation ability. The EKF fuses the inertial and optical flow measurement information for estimating the navigation error. Then, the estimated navigation error is used for correcting the SINS measurements. The results show that the integrated navigation algorithm can effectively reduce inertial navigation errors and isolate sensor failures. Some experiments will be done to test the performance of the fault-tolerant optical flow sensor/SINS integrated navigation scheme.
Data Availability
The datasets used or analysed during the current study are available from the corresponding author on reasonable request.
Conflicts of Interest
The authors declare that there is no conflict of interest regarding the publication of this paper.
Acknowledgments
This work was supported by the National Natural Science Foundation of China (NSFC) (Grant no. 61403007).