Tag: flight control

Integrated Navigation Systems Using Parallel Filtering

The authors look at the development of a robust navigation system employing a GNSS receiver, accelerometers, gyroscopes, magnetometers, an airspeed device and dead reckoning to supply a blended navigation solution to a flight control system on a small, unmanned aerial vehicle.

By Trevor Layh and Demoz Gebre-Egziabher

THE NUMBER FOUR has special significance to humankind.  According to Penelope Merritt (a Samuel Beckett scholar) “[f]our has long been a number of completion, stability and predictability, as well as the representation of all earthly things.” And so it is with navigation systems. There are four important requirements of any navigation system: accuracy, availability, continuity, and integrity. To quickly review:

Accuracy describes how well a measured value agrees with a reference value, typically the true value.

Availability refers to a navigation system’s ability to provide the required function and performance within the specified coverage area at the start of an intended operation.

Continuity is the ability of a navigation system to function without interruption during an intended period of operation.

Integrity refers to the trustworthiness of a navigation system. A system might be available at the start of an operation, and we might predict its continuity at an advertised accuracy during the operation. But what if something unexpectedly goes wrong? If some system anomaly results in unacceptable navigation accuracy, the system should detect this and declare that it can no longer be used for navigation at the expected accuracy level. GPS, for example, has built into it various checks and balances to ensure a fairly high level of integrity. The same may be said of other global navigation satellite systems. Satellite performance is continuously monitored and a satellite is set unhealthy when an anomaly is detected. Some receivers have built-in receiver autonomous integrity monitoring to detect and isolate problematic satellite signals and navigation support systems (such as the Wide Area Augmentation System) independently monitor the health of satellite signals and supply a timely warning in the case of anomalous signal behavior.

However, an aircraft, vessel, vehicle or some other platform still needs to be able to navigate if an independent primary navigation system becomes unavailable. This requires a back-up system of some kind and may take the form of an inertial navigation system, another radionavigation system such as eLoran, celestial navigation or just dead reckoning. Ideally, the platform’s navigation system should have multiple integrated sensors so that it continues to operate seamlessly even in the event of a sensor failure. We would call such a system robust. While we often use this word to describe a person with a strong healthy constitution, we can apply it to systems to refer to their ability to tolerate perturbations that might affect their functionality. A robust navigation system employs multiple sensors and uses appropriate filtering systems to autonomously detect anomalies, such as a failed sensor, and then to isolate it from the combined navigation solution.

It is important to catch navigation sensor failures early, ideally instantaneously, to reduce integrity risk as much as possible. This is not a trivial operation, and it requires clever software design and operation.

In this month’s column, we look at the development of such a robust navigation system employing a GNSS receiver, accelerometers, gyroscopes, magnetometers, an airspeed device and dead reckoning to supply a blended navigation solution to a flight control system on a small, unmanned aerial vehicle.

While the number four has special significance in religion, science and other aspects of our lives, the number five may be considered equally important — denoting, for example, how many digits we have on our hands and feet. For those mathematically inclined, it is the first safe prime number. And perhaps we should use it to more fully characterize a navigation system, denoting its accuracy, availability, continuity, integrity and robustness.

---

“Innovation” is a regular feature that discusses advances in GPS technology and its applications as well as the fundamentals of GPS positioning. The column is coordinated by Richard Langley of the Department of Geodesy and Geomatics Engineering, University of New Brunswick. He welcomes comments and topic ideas. Email him at lang @ unb.ca.

---

Multi-sensor navigation systems generate an estimate of a vehicle’s state vector by fusing information from a disparate set of sensors. In many instances the sensors used in these systems provide redundant information. For example, in GNSS receivers, more than four (the minimum number required) satellite measurements are used to generate a position, navigation and time or PNT solution. This redundancy is beneficial because it enhances accuracy. It also enhances integrity or robustness because it allows the detection and possibly the isolation of failed sensors. However, fault detection and isolation schemes do not work instantaneously because once a sensor has failed, it takes some time before this can be detected. This is especially true for failures that are drift-like in nature as opposed to step-like. Drift-like errors grow slowly and, thus, fault detection schemes that monitor filter residuals cannot detect them until they have grown to a point where they are sufficiently large to exceed preset thresholds.

The time between the onset of a fault and its detection — called the detection time — depends on the fault magnitude and thresholds of the fault detection algorithms. For a given fault magnitude, the length of the detection time represents a compromise between a navigation system’s continuity performance (or false alarm rate) and integrity risk (missed detection probability). The fact that faults cannot be detected instantaneously is an issue particularly for systems that have some form of dead reckoning (such as inertial navigation or velocity-based odometry) integrated with aiding sensors such as GNSS or radars. A failure in the aiding system (for example, a pseudorange fault in GPS) will lead to a corruption of the dead-reckoning solution. Once the GNSS fault has been detected and subsequently removed, the error induced by this failure has already propagated into the dead-reckoning solution. How does one deal with these types of errors? In this article, we discuss a solution to this challenge, which we call parallel filtering.

Solutions for dealing with the problem exist. For example, one approach that has been used is based on the idea of delayed measurements. In this approach, integration of aiding sensor measurements in the navigation solution is delayed until a period equal to the fault detection time has elapsed. If no faults are detected during this period, then the delayed measurements are extrapolated forward in time and integrated into the navigation solution. Alternately, we can rewind the dead-reckoning solution backwards in time, integrate the delayed measurements and fast-forward the integrated solution up to the current time epoch. While this approach works, it has several shortcomings, of which we will mention just two. First, it requires buffering sensor data. Second, the most current navigation solution is not as accurate as it can be, because it does not incorporate the most recent sensor measurements (that is, the delayed measurements). The parallel filtering approach and fault tolerance we describe in this article deals with both of these shortcomings. Of course, like any other engineering solution, it represents a compromise between competing requirements. We will discuss these compromises and their impacts later in the article. For now, we will concentrate on describing the mechanics of parallel filtering and its performance when implemented in an integrated flight control system used for navigation, guidance and control of small unmanned aerial vehicles or UAVs.

Parallel Filtering

To understand parallel filtering, consider the schematic in FIGURE 1, which represents the conventional way in which an integrated navigation system fuses the information from N sensors. All the measurements from the N sensors are integrated in a single sensor-fusion algorithm. In the context of what we are describing here, the algorithm consists of a navigation filter and a fault-detection filter. The sensor-fusion algorithm integrates the measurements from all N sensors and generates a single, optimal estimate of the navigation state vector.

In contrast to this, the schematic shown in FIGURE 2 is the parallel filtering approach introduced in this article. In this case, the same N sensors are divided up into M separate sensor clusters.

The measurements from the sensors in the jth cluster is processed in a sensor-fusion algorithm to generate an estimate of the state vector denoted x_j and a covariance matrix P_j. Each pair (x_j, P_j) is then sent to a blending filter that generates a single optimal estimate  and P. The estimate  is a weighted sum of the estimates from the M filters:

  (1)

where B_j are blending weights that function as switches, which can be “opened” (set to zero) to isolate a parallel filter momentarily or permanently when a failed sensor is detected. The analogy with a physical switch should not be taken literally, however, because they are not “hard on-off” switches. Instead, they are matrices, which serve to change the emphasis put on a particular parallel filter. The blending weights are calculated so that the estimate  is an unbiased minimum-variance estimate. In mathematical terms, this means that they minimize the trace of the final covariance P. We will give more detail on how to calculate the weights shortly, but before we do that, let us describe, at a high level, how all of this works.

Consider that one of the sensors in the th cluster fails. Theth fault detection filter will identify the fault and try to isolate it. If the fault is non-isolable, the th fault detection filter will raise an alarm. This can be done in various ways including inflation of the th filter covariance . An increasing covariance matrix  leads to a decreasing value of the corresponding blending weight  . For a non-isolable fault,   will eventually approach zero, which effectively isolates the th cluster from the navigation solution. If the fault was just a momentary glitch, then  and   are reset. In the simplest case,   can be reset to a weighted sum of remaining M-1 parallel state estimates. This is then blended with all of the other parallel estimates for generating the new . This does not require setting aside buffers to store delayed measurements. Neither does it require rewinding the solution back in time when recovering from a faulted sensor scenario.

Mathematical Formulation

Providing a detailed derivation of the parallel filter is beyond the scope of this short article. Instead, we will just summarize the steps in the parallel filtering algorithm with the key formulas that are used in determining the blending weights. For simplicity, we will assume that we are working with a system with two parallel filters (M = 2 in Figure 2). How this extends to systems with more parallel filters or complex interlinking between the filters will become apparent later in the article when we present the results from a case study.

To start, let us define some notation. We assume that the two parallel filters are extended Kalman filters (EKFs) generating estimates of the state vectors x₁ and x₂. We will denote these estimates  and . The covariances for these estimates are denoted by P₁ and P₂, respectively. The output of the blending filter is an estimate of the state vector x, which is a subset of x₁ and x₂. In mathematical terms, this means that we can define two mapping matrices M₁ and M₂ whose entries are either “1” or “0” and:

   (2)

The output of the blending filter  is, thus, given by:

. (3)

The blending weights are computed from:

  (4)

  (5)

where

  (6)

 (7)

. (8)

The covariance of  is given by:

(9)

where   and Π is given by:

(10)

where P₁₂ is the cross-correlation between the states of parallel filter #1 and #2. We will say more about this shortly. In the meantime, note that in Equation (9), P₁ and P₂ are the covariances computed by the parallel filters after the measurement update. This computation requires knowledge of K₁ and K₂, which are the EKF gains for parallel filters. The matrices H₁ and H₂ are the observation matrices for filters #1 and #2. They relate the measurements y₁ and y₂ of the two parallel filters to their respective state vectors as follows (refer to Figure 2):

y₁ = H₁x₁ + v₁   (11)

y₂ = H₂x₂ + v₂  (12)

where v₁ and v₂ are the measurement noises. Thus, the blending filter has to have knowledge of the measurement model and the gains of each parallel filter.

Finally, note that P₁₂ is zero if the dynamic models (time update equations) for the two parallel filters are completely independent. However, if they share sensors then there will be a correlation and P₁₂ ≠ 0. This is the case for the example we present later in this article. In this case, P₁₂ needs to be propagated between measurement updates. This can be done with the covariance time update equation (Lyapunov equation) for the joint state vector

.

Note that the architecture depicted in Figure 2 is meant to be a high-level depiction of the idea of parallel filtering. It should not be interpreted as an actual system architecture schematic. This will become apparent in the case study we present later in this article. The system we will consider there consists of three filters of which two are run in series (cascaded so that the output of the first is the input of the second) and each, in turn, is run in parallel with the third filter.

It is important to note that the proper blending of the various filters’ outputs hinges on an accurate estimate of the individual covariances. This is particularly true when a fault has occurred. An individual filter that has detected a failed sensor must inflate its covariance to reflect its faulted state. How a filter does this is the problem of fault-detection filter design and is beyond the scope of this short article. For the work presented here, we used fault-detection filters, which monitored the EKF measurement residuals to detect sensor faults. When these filters detected a fault, they immediately inflated the faulted sensor’s output noise covariance matrix. We cannot overemphasize, therefore, the importance of having a well-designed fault-detection filter that responds in a timely and accurate manner to sensor faults.

Case Study: Small UAV Flight Control

detection/isolation scheme described above, we discuss the results of a blending filter, which was used on the University of Minnesota UAV Laboratory Goldy flight control system (FCS) shown in FIGURE 3. The Goldy FCS is used for navigation, guidance and control of small UAVs. The results presented below were obtained by post-processing flight test data.

The architecture of the parallel filtering scheme used is shown in FIGURE 4. There are three separate filters whose outputs are blended: a GNSS-aided inertial navigation system (INS) filter, an attitude heading reference system (AHRS) filter and an airspeed-based dead-reckoning (DR) filter. Two blending filters are used to fuse the outputs from these three filters. The first blending filter fuses the attitude estimates from a GNSS-aided INS and an AHRS. The second blending filter fuses the position solutions from the GNSS-aided INS and the airspeed-based DR system. The AHRS and the airspeed-based DR filters are a pair of filters, which are cascaded to generate an estimate of the UAV navigation state vector. Thus, in the case of GNSS-denied operations, it can provide a position, velocity and attitude estimate to the flight control system. All of the sensors and software required to run these filters are part of the Goldy FCS. Before we present results of the parallel filter’s performance, we will briefly describe these three systems below.

The GNSS-aided INS uses a consumer/automotive grade inertial measurement unit (IMU) to generate a position, velocity and attitude solution at a rate of 50 Hz. A 1-Hz measurement update from GPS is used to arrest drift errors inherent in inertial navigation systems, especially those mechanized using low cost consumer/automotive grade sensors. The GPS position updates also allow estimation of the inertial sensor biases. The state vector for this GNSS-aided INS is denoted x₁ and consists of the following 15 states: latitude (Λ), longitude (λ), altitude (h), north velocity (V_n), east velocity (V_e), down velocity (V_d), roll angle (φ), pitch angle (θ), yaw angle (ψ), three gyro biases (b_p, b_q and b_r) and three accelerometer biases (b_ax, b_ay and b_az).

The second and third filters are a pair of estimators connected in series. The AHRS filter generates attitude estimates, which are fed to the airspeed-based DR filter. The AHRS uses the same IMU as the GNSS-aided INS to estimate roll (φ), pitch (θ) and yaw (ψ) attitude states of the vehicle as well as the three gyro biases (b_p, b_q and b_r). This AHRS filter’s six-dimensional state vector is denoted x₂. The attitude is then used to resolve airspeed measurements from the body frame of the UAV to the north-east-down coordinate frame. After adding an estimate of the local winds to this, a single integration yields a position solution. This is done at a rate of 50 Hz. A periodic 1-Hz update from GPS is used to arrest the inherent DR drift. It also allows estimation of the magnitude of the local winds. The state vector of the airspeed-DR is denoted x₃ and consists of the following 11 states: latitude (Λ), longitude (λ), altitude (h), local north wind speed (W_n), local east wind speed (W_e), yaw angle offset (Δψ), pitch angle offset (Δθ), three airspeed-measurement biases (U_b, V_b and W_b), and altitude offset (Δh).

In the UAV flight control system, the blended states of interest are position (Λ, λ and h) and attitude (φ, θ and ψ). This implies that four mapping matrices are required for the fusion. First, matrices are needed for the attitude blending using the GNSS-aided INS (M_1a) and the AHRS (M₂). Then, additional matrices are needed for the position blending using the GNSS-aided INS (M_1b) and the airspeed-based DR (M₃). The shaping matrices are given by:

   (13)

   (14)

   (15)

   (16)

where I_j×k is a j × k identity matrix and Z_j×k is a j × k matrix of zeros.

Filter Performance

Validation of the parallel filtering scheme was accomplished by post-processing data from a series of flight tests where the Goldy FCS was installed on a UAV flying around a box-shaped trajectory.

The first set of results was from a case where GPS was available from the moment the FCS is turned on until shortly after takeoff. Thus, GPS was available during initialization, take off roll and initial climb of the UAV. Then, GPS services were interrupted for a three-minute period during flight and restored shortly before the UAV landed. The GPS interruption was simulated by cutting out the 1-Hz measurement updates to the GNSS-aided INS and the AHRS/airspeed-DR system. In the background, however, there was another GNSS-aided INS that had an uninterrupted GPS service throughout the entire flight. This additional GNSS-aided INS solution is referred to as the reference solution and is used as ground-truth for assessing the performance of the parallel filtering scheme. For example, error plots shown below were generated by taking the difference between the various filtering schemes under consideration and this reference solution.

FIGURE 5 shows the errors in the attitude of all three filters during this flight test. It shows that the blended estimates of heading, pitch and roll tend to oscillate closer to zero error than either of the individual filters themselves. This is reflected in TABLE 1, where it can be noted that the root-mean-square (RMS) error of the blended solution is lower than either the GNSS-aided INS or the AHRS in each of the three attitude solutions.

FIGURE 6 shows the position errors of all three systems and illustrates one of the primary advantages of the proposed architecture. FIGURE 7 and FIGURE 8 show the blending weights matrices B₁ and B₂ before, during, and after the GPS outage. What is shown in these figures are the diagonal elements of these matrices.

The INS exhibits extreme drift errors after only three minutes of unaided operation. The blending algorithm detects this inaccuracy and places more weight on the slow-drifting AHRS-DR solution, as shown in Figure 8. When GPS services are restored, the GNSS-aided INS error is “reset,” and the position weights are re-established to their pre-outage levels with minimal transient responses.

We next show data from another flight test where an unplanned but fortuitous fault in the GPS sensor occurred. The cause of this fault has not been definitively determined, but potential reasons for it include loose cabling or outdated firmware. Nevertheless, this fault provided useful flight data for our architecture as no fictitious or simulated data was used. FIGURE 9 shows the GPS altitude measurements during this flight test. At t = 44 seconds a large oscillatory GPS error occurred. Similar errors were present in the GPS measurements of the velocities, latitude and longitude.

Thus, all filters were initialized and operated correctly for the first 44 seconds. Between 44 and 132 seconds, the GPS receiver output was in error. This time period corresponds to the taxi, takeoff and initial climb phase of the UAV’s flight. A “reference” GNSS-aided INS, which did not employ the fault detection and isolation scheme that was employed in the parallel filtering system, was running in real time for this flight test. However, the UAV was under manual control (fortunately). As shown by the gray solution in FIGURE 10, the “reference” (non-fault-tolerant) system running in the background diverged and never converged.

The dark traces in Figure 10 show the performance of the fault detection and isolation algorithm paired with the parallel filtering scheme described in this article. It is seen to be fault-tolerant and ignores the invalid measurements. Although nearly no aiding was provided until after the GPS sensor converged back to a stable state, the fault tolerant filter provided a much more accurate solution.

A bird’s eye view of the ground track of the UAV shows a similar trend. This can be seen in the position plot of FIGURE 11, which shows a roughly 60-second segment of the flight.

This north vs. east plot demonstrates that a non-fault-tolerant GNSS-aided INS provides an unstable position solution similar to the attitude shown in Figure 10. By contrast, the fault-tolerant system described in this article provides a smooth position estimate that ignores the “bad” GPS measurements and tracks the “good” measurements after they convergence back to the truth. Therefore, the safety of the aircraft would not have been in question, and the UAV could have completed multiple segments of fully autonomous waypoint navigation in spite of the faulty sensor measurements provided earlier.

Summary

The parallel filtering approach discussed in this article has the potential for providing a systematic way of designing multi-sensor navigation systems, which are robust to sensor faults. Unlike prior approaches, it obviates the need to maintain data buffers to store data, which can be played back in the event of a sensor fault. As noted earlier, like any engineering solution to problems, this one is a comprise between many competing requirements. As such, it has some drawbacks when compared to traditional approaches. We note two of them here as they are the focus of ongoing work. First, the computational overhead associated with this approach can be high especially if a large number of parallel filters are used. Thus, methods for streamlining the computations so that they are not computer-resource intensive will be important.

The second issue that needs further exploration is the way in which blending weights are computed. A key input to calculating the weights (as well as the “triggers” for the fault detection and isolation algorithm) are the covariances estimated by the various parallel filters. This can be problematic if the covariances used by the parallel filters do not match the true statistics. This can lead to turning off a particular filter when no faults had occurred or, worse, retaining a filter with a failed sensor in the blended solution.

For more detail about the Goldy FCS, go to www.uav.aem.umn.edu.

Acknowledgments

This article is based, in part, on the paper “A Fault-Tolerant, Integrated Navigation System Architecture for UAVs” presented at ION ITM 2015, the 2015 International Technical Meeting of The Institute of Navigation, Dana Point, Calif., January 26–28, 2015. The contents of this article reflect the views of the authors, who are responsible for the facts and the accuracy of the information presented herein. The authors acknowledge the United States Department of Homeland Security for supporting the work reported here through the National Center for Border Security and Immigration under grant number 2008-ST-061-BS0002. However, any opinions, findings, conclusions or recommendations in this article are those of the authors and do not necessarily reflect views of the United States Department of Homeland Security.

Manufacturers

The Goldy FCS uses a Hemisphere GNSS Crescent OEM board and an Analog Devices ADIS16405 iSensor MEMS inertial measurement unit.

---

Trevor Layh is a M.S. candidate in the Department of Aerospace Engineering and Mechanics at the University of Minnesota in Minneapolis. He obtained his B.S. in mechanical engineering from South Dakota State University, Brookings, S.D., and his research interests include backup navigation systems to GPS-aided inertial navigation systems.

Demoz Gebre-Egziabher is an associate professor in the Department of Aerospace Engineering and Mechanics at the University of Minnesota. His research focuses on the design of multi-sensor navigation systems. He holds a Ph.D. in aeronautics and astronautics from Stanford University, Stanford, Calif.

FURTHER READING

• Authors’ Conference Paper

“A Fault-Tolerant, Integrated Navigation System Architecture for UAVs” by T. Layh and D. Gebre-Egziabher in Proceedings of ITM 2015, the 2015 International Technical Meeting of The Institute of Navigation, Dana Point, Calif. January 26–28, 2015, pp. 702–712.

• Attitude Heading Reference System and Airspeed-Based Dead Reckoning Filters

Correlated-Data Fusion and Cooperative Aiding in GNSS-Stressed or Denied Environments by H. Mokhtarzadeh, Ph.D. dissertation, University of Minnesota UAV Laboratories, 2014.

“A Recovery System for SUAV Operations in GPS-Denied Environments Using Timing Advance Measurements” by T. Layh, J. Larson, J. Jackson, B. Taylor and D. Gebre-Egziabher in Proceedings of ITM 2015, the 2015 International Technical Meeting of The Institute of Navigation, Dana Point, Calif. January 26–28, 2015, pp. 293–303.

• UMN UAV Research Lab and Goldy Flight Control System

“Infrastructure” website, University of Minnesota UAV Laboratories, July 2014.

• Navigation in GPS-Denied Environments

Impact and Mitigation of GPS-Unavailability on Small UAV Navigation, Guidance and Control by D. Gebre-Egziabher and B. Taylor, Technical Report 2012-2, University of Minnesota, Department of Aerospace Engineering and Mechanics, November 2012. Available through online request.

• Avionics Reliability

Introduction to Avionics Systems, 2nd Edition, by R.P.G Collinson. Published by Kluwer Academic Publishers, Boston, Mass., 2003.

Civil Avionics Systems by I. Moir and A. Seabridge. AIAA Education Series. Published by American Institute of Aeronautics and Astronautics, Reston, Va., 2003.

• Example of a Fault-Tolerant Avionics System

“Performance of Honeywell’s Inertial/GPS Hybrid (HIGH) for RNP Operations” by  C. Call, M. Ibis, J. McDonald and K. Vanderwerf in Proceedings of PLANS 2006,  the Institute of Electrical and Electronics Engineers / Institute of Navigation Position, Location and Navigation Symposium, Coronado (San Diego), Calif., April 25–27, 2006, pp. 244–255, doi: 10.1109/PLANS.2006.1650610.

• GNSS Integrity

“Digging into GPS Integrity: Charting the Evolution of Signal-in-Space Performance by Data Mining 400,000,000 Navigation Messages” by L. Heng, G.X. Gao, T. Walter and P. Enge in GPS World, Vol. 22, No. 11, November 2011, pp. 44–49.

“Integrity for Non-Aviation Users: Moving Away from Specific Risk” by S. Pullen, T. Walter and P. Enge in GPS World, Vol. 22, No. 7, July 2011, pp. 28–36.

“The Integrity of GPS” by R.B. Langley in GPS World, Vol. 10, No. 3, March 1999, pp. 60–63.

• Multi-Sensor Systems

“Toward a Unified PNT — Part 1: Complexity and Context: Key Challenges of Multisensor Positioning” by P. D. Groves, L. Wang, D. Walter, H. Martin and K. Voutsis in GPS World, Vol. 25, No. 10, October 2014, pp. 18, 27–34, 47–49.

“Toward a Unified PNT — Part 2: Ambiguity and Environmental Data: Two Further Key Challenges of Multisensor Positioning” by P. D. Groves, L. Wang, D. Walter and Z. Jiang in GPS World, Vol. 25, No. 11, November 2014, pp. 18, 27–35.