Automated Tuning of the Nonlinear Complementary Filter for an Attitude Heading Reference Observer

Transcription

1 Automated Tuning of the Nonlinear Complementary Filter for an Attitude Heading Reference Observer Oscar De Silva, George K.I. Mann and Raymond G. Gosine Faculty of Engineering and Applied Sciences, Memorial University of Newfoundland, Canada. Abstract In this paper we detail a numerical optimization method for automated tuning of a nonlinear filter used in Attitude Heading Reference Systems (AHRS). First, the Levenberg Marquardt method is used for nonlinear parameter estimation of the observer model. Two approaches are described; Extended Kalman Filter (EKF) based supervised implementation and unsupervised error minimization based implementation. The quaternion formulation is used in the development in order to have a global minimum parametrization in the rotation group. These two methods are then compared using both simulated and experimental data taken from a commercial Inertial Measurement Unit (IMU) used in an autopilot system of an unmanned aerial vehicle. The results reveal that the proposed EKF based supervised implementation is faster and also has a better robustness against different initial conditions.. INTRODUCTION Estimation of attitude and heading (roll, pitch and yaw) of a body in three dimensions is a primary problem in autonomous vehicles operating in both underwater and aerial settings. Conventionally the problem is tackled using navigation grade Inertial Measurement Units (IMU) powered by Extended Kalman Filtering (EKF) approaches. As these systems are miniaturized for small scale or team deployment, limitations are faced with respect to payload, cost and availability. Low cost Micro Electro Mechanical Sensors (MEMS) are used to meet these constraints, while relying on simpler observers in place of the more resource demanding EKF. Observers based on the nonlinear complementary filter proposed by [] are viewed as a popular simpler alternative to the problem. As the accuracy of such an approach depends on the user tuned gains, the filter lacks the intuitiveness of the tuning process present in the standard stochastic methods, where gains are dependant on the variances of the sensors used. Therefore there exists a requirement for proper tuning approaches for optimal performance of the nonlinear complementary filter. Tuning of filter parameters (gains) in general are approached via numerous methods. These includes heuristic tuning [], analytical gain selection [2], gain adaptation [3] [4] and numerical optimization (both batch and recursive applications). The EKF for attitude estimation adapts the gains using optimality principles. For empirical selection of the process Q and sensor R noise filter parameters of the EKF, numerical variance estimation methods are in place during initialization of these systems [5]. Also, online identification of process and measurement noise variances are proposed using adaptive methods [3]. These stochastic adaptive formulations for observer gains, although powerful, are more suited for systems with high processing capability. This is due to the estimation of covariance states and calculation of matrix inverse. For low cost applications, deterministic nonlinear formulations are preferred for the attitude estimation problem. Reference [2] reports deterministic formulation of the invariant observers for low cost applications. The tuning of this nonlinear observer is performed analytically, by designing the gains using the observer dynamics. The absence of an analytical gain selection method for the nonlinear complementary filter proposed in [] requires the gains to be selected by the user. An automated approach to the problem is by numerical optimization of the filter tuning parameters with a supervisor providing estimates for tuning. Established nonlinear parameter estimation algorithms allow the problem to be addressed as a grey box system identification exercise [6]. Such numerical optimization approaches are particularly reported for automated gain tuning of various controllers [7] [8]. This work analyzes the application of nonlinear parameter estimation schemes for the complementary filter tuning problem. An EKF model reference tuning structure and an unsupervised error minimization method are implemented /2/$ /3/$ IEEE 7

2 for off-line automated tuning of the parameters. The main contribution of this paper is in the development of an automated tuning methodology for the filter parameter estimation of the nonlinear complementary filter. The proposed tuning methodology can readily be used in practical filter tuning of low cost autopilot systems. The designer can overcome the accuracy limitations of the heuristic trial and error tuning process by using the proposed tuning structure and the optimization scheme. The rest of the paper is organized in the following manner. Section 2 formulates the problem and summarizes the two observers used in the study, namely the EKF and the nonlinear complementary filter. Section 3 introduces the parameter estimation schemes with off-line batch estimation procedures. The proposed tuning scheme is applied on a simulated data set and an actual data set captured by a commercial IMU. Results are summarized in section 4. Performance in terms of robustness, speed of convergence, and complexity is discussed. 2. Considered Problem Following is a summary of the low cost attitude estimation problem and its solutions using the approaches of EKF and the nonlinear complementary filter. Quaternion parametrization is used in place of Euler or Direction Cosine Matrix (DCM) parameterizations, to achieve global parametrization of the body orientation while minimizing the number of parameters. Specific details of the attitude observer design used in the study is reported in [5]. 2.. Low Cost Attitude Estimation Problem The attitude estimation problem considered only uses inertial, gyroscopic and magnetic sensors for its solution. Additionally, it neglects the effects of earth rotation thus assumes a flat, non-rotating earth. Under this assumption the earth fixed frame can be considered an inertial frame which is used as the navigation frame for the problem considered. The kinematic and the sensor models used for simulation and observer design take the following form. 2.. Kinematic model q = 2 q ωb ib () In equation (), q is the unit quaternion representing the orientation of the body fixed frame with respect to the earth fixed frame and ωib b is the angular velocity vector of the body with respect to the inertial frame measured in the body fixed frame Gyroscope model ω m = ωib b + ω B + ν ω (2) ω B = ν B In equation (2), ω m is the measured signal from the gyroscope, ω B is the gyroscope bias term which is modeled as a random walk process and ν ω, ν B are measurement noises of the gyroscope, modeled as white noise Accelerometer model a m = a b ib + a B q g e q + ν a (3) ȧ B = ν ab In equation (3), a m is the measured signal of the accelerometer, a b ib is the acceleration of the body relative to inertial (earth) frame, measured in body fixed coordinate system, g e is the earth gravity reference vector, expressed in the earth coordinate system and ν a is the accelerometer measurement noise modeled as white noise. The accelerometer bias a B is not generally considered in the unaided attitude estimation problem, but is an essential element for solving the inertial navigation problem. As there are no additional sources to estimate the acceleration of the body a b ib, low acceleration motions are assumed and the term is absorbed in the sensor noise ν a Magnetometer model b m = B + q b e q + ν b (4) In equation (4), b m is the measured signal from the magnetometer, B is the magnetic disturbance experienced at the body frame due to actuators, cables, etc., b e is the earth magnetic field reference vector for the certain position of the earth expressed in the earth coordinate system and ν b is the magnetometer measurement noise modeled as white noise. For attitude estimation the model is simplified by absorbing the magnetic disturbance B in the noise term of the sensor model. Alternatively, the value of B occurring due to self-excited fields of the system can be found experimentally The Extended Kalman Filter The EKF is used as a supervisor to provide the automated tuner with reference trajectories. It is selected since the natural parametrization of the filter which only depend on the identifiable variance of the sensors. No additional fine tuning is performed on the filter in this study apart from setting the variances of the sensors to their provided values. The system model used for EKF formulation takes the following form. { ẋ } = { f(x, u, ν) = 2 q ωb q (ω } m ω B + ν ω ) ν B (5) 72

3 ȳ } = { h(x) } q = g e q + ν a b m q b e q + ν b { am The prediction correction structure of the EKF used for this study follows the equation set (7),(8). For computation the set of equations are discretized by first order expansion and the quaternions are normalized at each update step. P rediction F = f(x, x u, ν) x=ˆx,ν=ˆν= G = f(x, ν u, ν) x=ˆx,ν=ˆν= ˆq = 2 ˆq (ω m ˆω B ) ˆω B = P = F P + P F T + GQG T ŷ a = ˆq g e ˆq = ˆq b e ˆq ŷ b Correction H e K ˆx P = h(x) x x=ˆx = z ŷ a = P H T (HP H T + R) = ˆx + Ke = P KHP In equation set (7) and (8), P is the state covariance matrix, x is the state vector, z is the measurement vector, F,G,H are linearizations of the nonlinear state space equations at ˆx, K is defined as the Kalman gain matrix, Q is defined as the process noise matrix and R is defined as the measurement noise matrix. It is important to note that the EKF is the defacto standard for nonlinear estimation, although several improved versions of the filter are reported in literature. The linear quaternion error representation is modified to nonlinear multiplicative error term in the Multiplicative Extended Kalman Filter (MEKF) [9]. The Uncented Kalman Filter (UKF) uses nonlinear mapping of carefully selected sample points to estimate the probability distribution []. The Invariant Extended Kalman Filter (IEKF) proposed by [4] exploits symmetry which leads to a class of filters with a larger domain of convergence. All of these are candidate supervisors for the filter parameter adaptation method proposed. The IEKF in particular would be a strong supervisor due to its convergence characteristics and the symmetry preserving formulation as in the nonlinear complementary filter Nonlinear Complementary Filter The nonlinear complementary filter uses a deterministic approach to the attitude estimation problem. For this study we used the quaternion formulation of the explicit complementary filter as proposed in []. Figure 2 illustrates the (6) (7) (8) basic structure of the filter. When compared with a Luenberger observer structure, the nonlinear complementary filter performs a partial state update (only ω B state) using a fixed gain. But the filter differs in its output error calculation, which is nonlinear and exploits symmetry of the Special Orthogonal group in 3 dimensions (SO(3)), enabling good convergence characteristics. P rediction ˆ q = 2 ˆq (ω m ˆω B ) ŷ a = ˆq g e ˆq = ˆq b e ˆq ŷ b (9) Correction e = w (ŷ a a m ) + w 2 (ŷ b b m ) ˆω B = K p e + K i edt () Where w and w 2 are weights given to errors with reference to the accelerometer estimate and the magnetometer estimate, respectively. The terms K p and K i are the proportional and integral gains of the filter. The gains of the nonlinear complementary filter are selected by the user. But for low cost applications it is often the case that there is no reference for these filter parameters to be set to optimal values. In practice it would be advantageous if methods are in place to tune the values using some relevant objective function. 3. Parameter Estimation Method This study attempts to apply general nonlinear parameter estimation methods for tuning of the nonlinear complementary filter. The filter runs on an embedded system with relatively low processing capability, so the application of the nonlinear parameter estimation methods require the process to be done off-line on a powerful system. This tuning process would provide filter parameters to be plugged in for operation and also provide reference values for online filter tuning algorithm development. Most techniques rely on steepest decent based and Newton-based methods. To exploit both characteristics of robustness of the steepest decent and convergence of Newton s method, the Levenberg Marquardt(LM) algorithm is used. The LM method was found to meet the sufficient performance in this particular application, so other algorithms such as trust region based methods and global optimization schemes (simulated annealing, genetic algorithms) were not explored. 3.. Levenberg Marquardt Algorithm The LM algorithm selects between Gauss Newton direction and the steepest decent direction for parameter update. The algorithm controls the transition between the two methods and the update step of gradient decent by a damping factor µ. Because of the nonlinear nature of the observer 73

4 gyro m acc m mag m K p K i Non-Linear Complementary Filter Extended Kalman Filter Gain Tuning Algorithm ˆq NCF ˆq EKF Figure. EKF model reference supervised tuning structure + - Tracking error (deg) mean and variance ACC EKF Tuner Tuner 2 acc m mag m gyro m + - ˆω B Non-Linear Complimentary Filter 2 ˆq (ω m ˆω B )dt K i K p y acc ˆ = ˆq g e ˆq y mag ˆ ˆq b e ˆq edt error ˆq NCF Pitch Roll Yaw Figure 3. Comparison of the tracking results of simulated dataset. ACC-estimate from using only the accelerometers and magnetometers, EKF- estimates form the extended kalman filter, Tuner - tuned complementary filter using supervised structure, Tuner 2- tuned complementary filter using the unsupervised structure Gain Tuning Algorithm Figure 2. Unsupervised self tuning structure update rule, numerical methods are used for Jacobian calculation. It is found by experiment that the calculated Jacobian at each data point tends to degrade the robustness of the algorithm. Rather, a Jacobian of the total error of the data set is found. The parameters of the standard LM algorithm [6] were set so the maximum iterations is 5, initial parameter estimates are, the initial trust parameter is 3 and update tolerance for convergence checking is EKF model reference supervised tuning The first approach uses reference trajectory estimates provided by an EKF on the training data set. Figure illustrates the structure of the tuning method proposed. The quaternion reference trajectory is compared against the trajectory provided by the nonlinear complementary filter to form an error signal. Instead of the linear error terms for the quaternions, the more accurate multiplicative error term can be used in the algorithm. The linear formulation was found to be sufficient for the purpose. Optimization is then performed using the squared error of this signal as the cost function Unsupervised error minimization self tuning The second approach uses the error calculated by the filter itself as the signal to be minimized in a least squares sense. This unsupervised method is illustrated in Figure 2. This method relieves the computational overhead of applying an EKF for reference trajectory generation. In both approaches it is found that the Jacobian calculated at each data point is ill conditioned, therefore a Jacobian is numerically calculated on the whole data set. 4. Results The proposed tuning method was used for tuning of a simulated dataset developed using the models reported in Section 2. It was then performed on an actual dataset captured by a commercial IMU, VectorNav VN(T). 4.. Simulated results Simulations were performed to validate the convergence of the tuning structures proposed. Both the tuning structures were iterated with the same parameters. The optimized tuning parameters were able to achieve close performance to that of the EKF estimates. The robustness of the two methods was tested and verified for different parameter initializations. The convergence efficiency of the supervised method was better in terms of elapsed time and the damping parameter update of the LM algorithm. Figure 3 illustrates the results of the two tuning structures where both structures successfully produces accurate results. The minimum performance level expected from the filter is illustrated as ACC in Figure 3. These are the estimates gathered by only utilizing the accelerometer and the magnetometer. This estimate is obtained by solving for the rotation matrix which corresponds to the two vector observations in the two frames (Wahba s problem) using singular value decomposition [] Experimental results The verified tuning algorithms were used to tune the nonlinear complementary filter for an experimental data set captured by the VN(T) commercial IMU. The IMU was 74

5 subjected to a random motion profile manually covering the full range of motion in terms of roll, pitch, yaw values and samples of data (accelerometer, gyroscope and magnetometer measurements) were collected at a data rate of 3Hz. This data set was used for tuning purposes employing the proposed automated tuning methodology. The VN(T) IMU posses onboard high speed signal processing which fuses the measurements from the sensors using a dedicated EKF which incorporates fully calibrated and temperature compensated sensor models. The estimates given by the onboard electronics at the high data rate of 3Hz was assumed as ground truth data for comparison of results. Both the tuning structures were implemented where the unsupervised approach optimizes the parameters using the measurement prediction error as a cost, while the supervised approach first implements an EKF based on the noise variances of the sensors and reference it for parameter optimization. The convergence of the tuned parameters were checked for various parameter initializations of the filter. Iteration Cost Update K p K i Table. EKF supervised tuning iteration steps of the experimental data set. Elapsed time is 9.2 seconds. During parameter estimation, to better handle the nonlinearities of the system, updates were performed so that the K p filter parameter converges before the K i update steps are attempted. This allows the optimization to be stable at distant parameter initializations. To generalize the applicability of the system the initial estimates are set to zero in the generated results. Tables and 2 presents the iterations performed in the supervised and the unsupervised tuning structures presented in this study. Figures 4 and 5 illustrates the performance of the two tuning structures. The EKF supervised tuning structure proposed in this study was observed to give better tuning parameters, and its convergence was faster than the unsupervised method. The faster convergence was a result of Gauss Newton iterative steps which it appears to take during the process. The unsupervised method similarly solves Tracking error (deg) mean and variance Pitch Roll Yaw ACC EKF DC DC2 Figure 4. Validation results - Comparison of the tuned complementary filter using the supervised approach against EKF performance on an actual data set. ACC-estimate from using only the accelerometers and magnetometers, EKF- estimates from the extended Kalman filter, DC- Complementary filter with only parameter K p, DC2- Complementary filter with two parameters K p, K i Iteration Cost Update K p K i Table 2. Unsupervised tuning iteration steps of the actual data set Elapsed time is 6 seconds. the least squares optimization with respect to the available information, but the updates were more based on damped gradient direction. This slows down the convergence of the solution. The EKF supervised method also appeared robust to various initial conditions. Initialization of the gain parameter was varied from to 5 to verify robustness in the neighborhood. Both the methods were successful in tuning the nonlinear complementary filters although EKF supervised method is preferred for speed and robustness. 5. Conclusion and Future work This paper reported the application of nonlinear parameter estimation methods for nonlinear complementary filter tuning. Parameter estimation scheme utilized a Levenberg Marquardt algorithm with modifications to the Jacobian calculation and sequential update of parameters. The following two tuning structures were proposed and experimented. 75

6 Tracking error (deg) mean and variance Pitch Roll Yaw ACC EKF DC DC2 Figure 5. Validation results - Comparison of the tuned complementary filter using the unsupervised approach against EKF performance on an actual dataset. ACC-estimate from using only the accelerometers and magnetometers, EKF- estimates from the extended Kalman filter, DC- Complementary filter with only parameter K p, DC2- Complementary filter with two parameters K p, K i Roll(rad) Pitch(rad) Yaw(rad) Reference data Tuned NCF Data point Figure 6. Tracking performance of the Tuned nonlinear complementary filter using supervised method on a validation data set captured by the Inertial Measurement Unit Unsupervised error minimization structure: The error signal of the nonlinear complementary filter itself is minimized in a least squared sense. This self tuning method does not require additional supervisor for tuning purpose. EKF supervised tuning structure: An EKF attitude estimator is applied on a training data set and the output quaternion was used as the reference signal for nonlinear complementary filter parameter tuning. This method exhibits faster and more accurate tuning of the filter parameters. The EKF supervised structure is recommended for nonlinear complementary filter tuning. The algorithm can be readily applied for low cost autopilot system development by replacing the ad-hoc tuning procedures which are used for the nonlinear complementary filter. This study is expected to be developed to online parameter estimation of the filter parameters. Furthermore, it is expected to extend the application of nonlinear system identification to applications entailing model reference inertial navigation schemes. References [] R. Mahony, T. Hamel, and J.-M. Pflimlin, Nonlinear Complementary Filters on the Special Orthogonal Group, IEEE Transactions on Automatic Control, vol. 53, pp , June 28. [2] P. Martin and E. Salaün, Design and implementation of a low-cost observer-based attitude and heading reference system, Control Engineering Practice, vol. 8, pp , July 2. [3] R. Mehra, On the identification of variances and adaptive kalman filtering, Automatic Control, IEEE Transactions on, vol. 5, pp , apr 97. [4] S. Bonnabel, P. Martin, and E. Salaun, Invariant extended kalman filter: theory and application to a velocity-aided attitude estimation problem, Decision and Control, 29 held jointly with the 29 28th Chinese Control Conference. CDC/CCC 29. Proceedings of the 48th IEEE Conference on, pp , 29. [5] J. Farrell, Aided navigation: GPS with high rate sensors. Mcgraw-hill, 28. [6] J. Nocedal and S. Wright, Numerical optimization. Springer verlag, 999. [7] S. Woodward and D. Garg, A numerical optimization approach for tuning fuzzy logic controllers, Systems, Man, and Cybernetics, Part B: Cybernetics, IEEE Transactions on, vol. 29, pp , aug 999. [8] A. Varsek, T. Urbancic, and B. Filipic, Genetic algorithms in controller design and tuning, Systems, Man and Cybernetics, IEEE Transactions on, vol. 23, pp , sep/oct 993. [9] F. Markley, Attitude error representations for kalman filtering, Journal of guidance, control, and dynamics, vol. 26, no. 2, pp. 3 37, 23. [] J. Crassidis and F. Markley, Unscented filtering for spacecraft attitude estimation, Journal of Guidance Control and Dynamics, vol. 26, no. 4, pp , 23. [] F. Markley, Attitude determination using vector observations and the singular value decomposition, Journal of the Astronautical Sciences, vol. 36, no. 3, pp ,