Kalman Filter Enhancement for UAV Navigation Roger Johnson* Jerzy Sasiade** Janusz Zalewsi* *University of Central Florida Orlando, FL 86-2450, USA **Carleton University Ottawa, Ont. KS 5B6, Canada Keywords Kalman filter, UAV navigation, fuzzy logic, parallel computations, parameter estimation. Abstract his paper proposes two methods to enhance traditional extended Kalman filter for UAV navigation. One is based on using fuzzy rules to choose parameters of an adaptive Kalman filter. he other uses inherent parallelism to speed up iterations in Kalman filter computations. Both methods are described briefly and simulation results are presented. INRODUCION Unmanned Aerial Vehicles (UAV), such as spacecraft, aircraft, helicopters, free-flying robots or mobile robots are increasingly applied in various domains, particularly in the military, scientific research, and in certain industries. herefore it becomes crucial to optimize trajectories, motion, fuel consumption and other performance related aspects of UAVs. In navigation of a UAV, three mutually dependent issues need to be considered: plan the path dependence on the nown information, determine the position and velocity of the vehicle, and avoid the unexpected obstacles. In general, obstacles may not be fully nown when we plan the path for the UAVs. In this case, the vehicle has to handle an event such as unexpectedly encountering some moving or static obstacles on its way and an original plan may no longer be executable in the new situation. his is especially important when the vehicle operates in the space environment for long periods of time, without frequent communications with the ground station. From this point of view, as well as because of the need of fault-tolerant operation, a new set of requirements emerges calling for autonomous operation of UAVs for long unattended periods of time. For this purpose, our group is currently developing an Autonomous ealth onitoring and Control System (ACS), based on the new concept of a igh-fidelity Dynamic odel-based Simulation (FDS). It relies on the use of highly accurate dynamic models to replicate the behavior of the actual system [4]. Desired Objective Conventional Real-ime Controller actuator Real-time Nonlinear Estimation Robust and Dynamic Fault Detection odule Optimal Filter Disturbances and Uncertainties odeling Based Reasonning Nonlinear Robust Control Real-time Nonlinear Estimation Robust and Dynamic Fault Detection odule Optimal Filter lant Figure. Basic architecture of the control system. he basic architecture of the entire system is presented in Figure. Out of several layers of the control hierarchy, in this paper we consider only the lowest, monitoring layer. he essential function of the control system at this layer is to monitor both the state of the vehicle itself and the state of the actuators. When the vehicle has to change its original path, and revise its motion to achieve the collision-free path during navigation, the environment uncertainty and complexity is a ey issue. he position and velocity of the vehicle can be determined, when navigating and guiding an autonomous vehicle, with the Global ositioning System (GS). It is nown, however, that several errors are associated with the GS measurement [6]. It has superior long-term error performance, but poor short-term accuracy. For many vehicle navigation systems, GS is insufficient as a standalone position system.
o ensure high accuracy and fidelity of monitoring, which in principle means detecting any unexpected behavioral changes, in real time, we use Kalman filtering [2]. Kalman filtering is a form of optimal estimation characterized by recursive evaluation, and an internal model of the dynamics of the system being estimated. he dynamic weighting of incoming evidence with ongoing expectation produces estimates of the state of the observed system. We propose to enhance an extended Kalman filter in two ways. First, we apply fuzzy rules to the weighted Kalman filter to increase accuracy. Secondly, we apply parallelization to the extended Kalman filter to achieve high computational speed. In the next sections, we present the principles of both approaches and discuss the simulations. FUZZY ADAIVE KALAN FILER o ensure high accuracy and fidelity of monitoring, we use data fusion to combine measurements from GS and Inertial Navigation System (INS). he integration of GS and INS is ideal for vehicle navigation. In general, the shortterm accuracy of INS is good and the long-term accuracy is poor. he disadvantages of GS/INS are ideally cancelled. If the signal of GS is interrupted, the INS enables the navigation system to coast along until GS signal is reestablished []. he requirements for accuracy, availability and robustness are therefore achieved. In this paper, a fuzzy logic adaptive system (FLAS) is used to adjust the exponential weighting of a weighted EKF and prevent the Kalman filter from divergence. he fuzzy logic adaptive controller (FLAC) will continually adjust the noise strengths in the filter s internal model, and tune the filter as well as possible. he FLAC performance is evaluated by simulation of the fuzzy adaptive extended Kalman filtering scheme of Fig.2. he models and basic implementation equations for the weighted Kalman filter are shown below, for the nonlinear dynamic model and the nonlinear measurement model: x = f ( x, ) + w + z = h( x, ) + v where w ~ N (0,Q) and v ~ N (0, R). We assume model covariance matrices equal to: where, α, and Q and R are constant matrices.. For α>, as time increases, the R and Q decrease, so that the most recent measurement is given higher weighting [5]. If α=, it is a regular EKF. R = Rα Q = Qα 2 ( + ) 2 ( + ) () (2) (3) (4) INS Estimated INS errors seudo-range Corrected position, velocity,etc Residuals GS FLAS redicted measurements Figure 2. Fuzzy adaptive extended Kalman filter. a EKF Computation of the Kalman gain and covariance matrices has been described in [6]. ere we concentrate only on the fuzzy rules simulations. A good way of verifying whether the Kalman filter is performing well is to monitor the residuals (innovations). hey can be used to adapt the filter. he covariance of the residuals and mean values of residuals are used to decide the degree of divergence. he value of covariance relates to R. If the residual has zero mean, covariance is as follows: = + R z he purpose of a fuzzy logic adaptive controller is to detect the bias in the measurements and prevent divergence of the extended Kalman filter. It is usually assumed that the process noise w is white, but sometimes the process noise could be correlated with itself, non-white, which causes convergence problems. By selecting right α, the fuzzy controller optimally adapts the Kalman filter and tries to eep the innovation sequence act as zero-mean white noise. FLAS SIULAION RESULS he membership functions for fuzzy control are shown in Figures 3-5, for covariances, mean value and α. he FLAC uses nine rules shown in able, for example: If the covariance of residuals is large and the mean values are zero hen a is large. If the covariance of residuals is zero and the mean values are large hen a is zero.
zero small large 0 (4) 2.(4) 2 z /R [m 2 ] Figure 3. Covariance membership functions. zero small large 0 2 20 ean [m] logic adaptive EKF almost agrees with the theoretical error covariance. ultiples of Q and R in the table (5Q, 2R) mean that the real-time parameters are 5 and 2 times as large as the designed Q and R. able 2. Comparison of theoretical and actual error covariance for X-axis. Q R heory Actual 5Q R 3.7 3.392 5Q 2R 5.93 5. 3Q 2R 4.6896 4.8469 5Q 4R 8.962 8.2 Figure 4. ean value membership functions. zero small medium large 300.02..2 a Figure 5. a membership functions. able. Rules for the FLAS (L large, medium, S small, Z zero). a ean Value Z S L Z S Z Z S Z L L L Z he state variables used in simulation are: position and velocity errors of the INS East, North, Altitude, GS range bias and range drift. he designed standard deviation of GS measurement R is 5 [m]. he designed standard deviations of Q for INS are 0.002 [m], 0.002 [m], and 0.002 [m] for the East (x), North (y), and Altitude (z) respectively. Results of the simulations are presented in able 2 and Figure 6, for X axis only, to save space. hey show that after the filter stabilizes, the actual error covariance of fuzzy Variance x 200 00 0 0 00 200 300 400 500 600 700 800 900 000 ime (s) Figure 6. Actual (solid line) and theoretical (dashed line) covariances (X axis) for 5Q and 2R. ARALLELIZAION OF KALAN FILER Convergence problems, which occur in discrete Kalman filter computations discussed in previous sections, may be eliminated if a more accurate, continuous Kalman filter representation is used. In this case, however, the Kalman filter algorithm is very computationally intensive, therefore conventional real-time implementation using a single processor system is often not satisfying. We have experimented with parallel processing of both the regular Kalman filter and the extended Kalman filter. he exact mathematical model representing the motion of the vehicle s center of mass is described by the equations presented in [4]. Below we only discuss its parallelization. For any spacecraft vehicle, there are two types of parameters that are involved in the controller design for a navigation system design: position and attitude. Nonlinearities exist in both cases, however, a linear model is good enough for the attitude controller design. Below we discuss parallelization strategies for both the linear attitude model and the non-linear navigation model.
he Kalman filter equation can be split into two parallel groups, state and covariance, connected by Kalman gain K and the measurement matrix. herefore, the computation tas for Kalman filter can be split into two sets: state and error covariance equations. his is a quite straightforward parallelization method, by taing two processors, one doing state update and the other doing error covariance update. owever, the computations of the covariance equation are approximately three times more computationally demanding than state equations. herefore, one should loo for a parallel implementation of an algorithm that significantly reduces the processing time for the covariance equations. In our case study [3], the covariance matrix (6 6) and the measurement matrix (3 6) can be partitioned into (3 3) submatrices, which allows partitioning of the Kalman gain matrix into (3 3) submatrices, each of which can be calculated independently. Based on the above partitioning method, we can map the computation of the submatrices into multiple tass and let them executed in parallel. Due to the symmetry of matrices and, both of them have 3 submatrices to be computed. ence, 3 processors (tass) can be applied to carry out this computation. owever, in computing the matrix all of the 3 submatrices of are needed, which increase the inter-tas communication time. he partitioning of the Kalman gain gives us a hint that we can tae 2 processors, one to computes K and the other to computes K 2. Based on this idea and the study of partitioned equation, we can distribute the computation as follows: as: Compute K, and and compute the state estimation and prediction. as2: Compute K 2, 2, 22 and 2, 22. For extended Kalman filter (EKF), the same partition method as above can be used. owever, in this point-topoint navigation application, we are taing the continuous time model and in time-update phase the error covariance is computed. o perform the integration for, the system state at every integration point should be nown. his prevents the possibility of executing the state and the covariance (in time update phase) separately on two processors. But we can still apply the partition strategy described above. Since we are measuring 2 state variables, the measurement Jacobian matrix is 2 6 and can be factorized into three 2 2 submatrices. [ 0 ] = 0,2 2 2 2 2 2 Defining the predicted covariance as and filtered covariance as, and dropping all the time indices and bracets which indicate functions for simplification, hen the matrices are partitioned into 2 2 as follows: = = = = hen in the Kalman gain equation, the matrix product becomes ence the Kalman gain equation becomes ( = ( 2 3( We see that the Kalman gain K is split into 3 submatrices, each of them can execute independently. So this partitioning method can be mapped onto 3 processors. Based on this idea and the study of equation, the computation is distributed as follows, with as as master tas and the remaining two as slave tass: as: Compute K, compute, compute estimated state X e, by, compute predicted state X - e,+, and the predicted error covariance. as2: Compute K 2 and compute 2, 22. as3: Compute K 3, compute 3,, 33 and compute predicted covariance - + 2 3 2 22 3 = 2 3 = 0 0 2 = 3 2 3 2 33 [ ] K = ( + R ) + + + R R R ) ) ) = 2 3 22 K = K 2 K 3 3 33 ( + R )
ARALLEL SIULAION RESULS he parallel simulations have been implemented in ANSI C, on a VEbus multiprocessor computing system under the real-time operating system VxWors with multiple general purpose processor boards, VE67 (Fig. 7). Figure 7. ultiprocessor VE-based system. Unix Server Unix Station he spacecraft we consider is a gravity gradient stabilized spacecraft with a circular orbit. he altitude r, inclination angle i, physical parameter and initial conditions are given in able 3. Since we were doing real-time simulation, the sensors were actually not available. We integrated the first order system equation and obtained the state variable value at each sampling period and superimposed a random number with certain standard deviation. Figures 8 illustrates the simulation results for the pitch angle. he timing comparisons for sequential and parallel versions are shown in able 4. Ethernet roces sor 0 roces sor roces sor 2 Shared Local Local VEbus he boards support shared memory through the VEbus. Each of them contains a C68040 processor and an Ethernet interface that supports C/I. We conducted simulation of extended Kalman filter on a single processor, regular Kalman filter on a single processor, extended and regular Kalman filter on multiple processors and square-root Kalman filter on single processor. able 3. hysical parameters and initial conditions. Figure 8. Estimated and measured pitch angle. able 4. Computation time (in microseconds) for sequential and parallel implementations. Filter type Sequential timimg arallel timing Regular 6,360 6,000 Extended 3,560 8,338 oment of inertia Initial angle Orbital parameter I x =20Kgm 2 I y =20Kgm 2 Iz=.2Kgm 2 θ=0. rad φ=0. rad ψ=0. rad r=352km i=28.5 ω 0 =0.004 rad Square root 9,650 N/A CONCLUSION he fuzzy adaptive Kalman filtering has been proposed for guidance and navigation of mobile robots, especially for 3-D environment. he regular extended Kalman filter requires high number of states for accurate navigation and
positioning and is unable to monitor the changing parameters. A fuzzy logic adaptive system (FLAS) is used to adjust the exponential weighting of a weighted EKF and prevent the Kalman filter from divergence. he fuzzy logic adaptive controller (FLAC) will continuously adjust the noise strengths in the filter s internal model, and tune the filter as well as possible. he FLAC performance has been evaluated by simulation of the fuzzy adaptive extended Kalman filtering scheme. he FLAC requires smaller number of states for the same accuracy and therefore it would need less computational effort. Alternatively, the same number of states (as in regular filter) would allow for more accurate navigation. owever, what is often the case in UAVs, if loss of accuracy due to discretization cannot be tolerated, continuous state equations have to be used. In such case, extended Kalman filter calculations can tae advantage of the relative independence of state and covariance matrix updates, which leads to parallelization of computations. he test results show that by adopting the partition method described in this paper, a time reduction in the standard Kalman filter is gained. For the extended Kalman filter, however, the parallel execution is slower than the sequential version. his occurs because the numerical integration of the state projection phase increases substantially the necessary computations, hence increases significantly intertas communication and synchronization. Future wor will involve investigation of more effective parallelization methods and combination of both approaches discussed in this paper. REFERENCES [] Brown R.G.;.Y.C. wang. 997. Introduction to Random Signals and Applied Kalman Filtering. John Wiley and Sons, New Yor. [2] Gelb A. (Ed.). 974. Applied Optimal Estimation. he I ress, Cambridge, ass. [3] Johnson R.; S. Jayaram; L. Sun; J. Zalewsi. 2000. Distributed rocessing Kalman Filter for Automated Vehicle arameter Estimation: A Case Study, roc. IASED Int l Conf. on Applied Simulation and odeling, pp. 270-276. [4] Johnson R.; Z. Qu; S. Jayaram; Y. Jin. 2002. Autonomous Spacecraft Vehicle ealth onitoring and Control System Based on Real-ime odel-based Simlation, submitted for publication. [5] Lewis F.L. 986. Optimal Estimation with Introduction to Stochastic Control heory. John Wiley and Sons, New Yor. [6] Sasiade J.Z.; Q. Wang. 999. Sensor Fusion Based on Fuzzy Kalman Filtering for Autonomous Robot Vehicle, roc. 999 IEEE Int l Conf. on Robotics and Automation, pp. 2970-2975.