Home

5 MB PDF - BYU MERS Laboratory

1. 1 1 1 50 100 150 200 250 300 350 450 00 3 2r 7 ce c SO eh hh hh amp _2 f f i f f f i 50 100 150 200 250 300 350 400 450 500 ET Lu 1 1 y 1 h 1 1 50 200 250 300 350 400 450 500 Time s Error o en up een in ame y Zn ms 2 I B od o 2 1 00 1 50 200 250 300 350 400 450 500 Ss r r r r r r r r r r 2 or 4 o co XS o a ba p me Yr Ha d d 1 00 1 50 200 250 300 350 400 450 500 2 4 B o Lu 2r 1 00 1 50 200 250 300 350 400 450 500 Time s Non centripetal Acceleration in the z Direction m s 5 7 T 7 T r 7 T T E c 5 i n i 50 100 150 200 250 300 350 400 450 500 5 r r r r r r T T r r E A A ET TTT ET 5 1 1 50 100 150 200 250 300 350 400 450 500 5 r 7 r r r 7 5 1 1 i h y 50 100 150 200 250 300 350 400 450 500 Time s SR UKF Solution o Error Figure 5 6 Truth data and the SR UKF solutions for non centripetal acceleration in the x y and z directions See text and Table 5 3 for analysis 72 Phi deg 40 T T T T T T T T T T 20 E c B o m m m 20 40 f 1 1 1 f 1 50 100 150 200 250 300 350 400 450 500 Ss 40 r r r r r r r 3 20r LL O gt 20 4 cc 75 40 1 1 1 1 1 1 1 1 1 5
2. 0 46 4 35 4 36 4 37 4 38 4 39 Ke Pod Sl S 4 40 ii Bai KilYmeasured iti 1 4 41 USK Si 4 42 Sj cholupdate Sii U 4 43 The output estimate g is computed by passing 1 through the nonlinear func tion h Next a set of output points 1 is calculated by passing each of the new sigma points X4 1 through the nonlinear output function h A similar two step approach is applied to the calculation of the Cholesky factor Syi of the observation error covariance in Equations 4 37 and 4 38 where R is the measurement noise co variance Two nested inverse or least squares solutions are used to calculate the Kalman gain K in Eq 4 40 Finally the posterior measurement update of the Cholesky factor of the state covariance is calculated in Eq 4 43 A MATLAB script used to calculate the measurement update is included in Fig 4 3 4 3 4 Nonlinear State Functions Now that we have sufficiently set up the Kalman filter to optimize our mea surements we introduce the functions that the filter operates with Using Eq 4 12 we can construct f If we represent Eq 4 12 as the time derivative of the state E then fl i Ada 4 44 where A is the change in time or sample rate This is an approximation of the nonlinear function f If A is sufficiently small then the approximation good The function f must be made to accept large arrays of points so in the following MATLA
3. 080084 44 Conclusion se cr esgarar tek ditey adaa eh 5 Results bl Flight Simulator ess es esa E AA 5 2 Kalman Filtering the Simulated Data 5 3 Kalman Filtering Actual Flight Data 5 3 1 Measurements of the Entire Flight 5 3 2 A Smaller Portion of the Flight 5 4 Filtered Results ss escenas ee Pa ee ba bee lee 5 5 Conclusion mn 6 Conclusion 6 1 Contributions 24 s 6 2 Future Work Bibliography A Hardware Implementation A 1 Basic Operation of the TS 7800 2 2 22 lll A2 COM Connections 9 44 rar 4444 das ai A 3 MOTRON Operating Source Code len A 4 Additional Resources 2 2 m mn nn xvii 55 95 59 63 64 66 67 68 85 86 86 88 91 xvili List of Tables 3 1 IMU Scale Factors ica ea a A AA 30 3 2 Statistics of Measurements shown in Figures 3 9 and 3 10 33 5 1 Northing Easting and Altitude Errors m 60 5 2 Body Frame Velocity Errors m s llle 61 5 3 Non centripetal Acceleration Errors m s 62 5 4 Attitude Errors cioe a eee cR Bae AAA A 62 5 5 Angular Hate Errors rad s 2 2 oue RR A RR CA oe enger 63 5 6 Difference Between SR UKF and NovAtel Position m 64 5 7 Difference Between SR UKF and NovAtel Attitude 65 5 8 Difference Between SR UKF and NovAtel Position m 66 5 9 Diff
4. ainc qu rv afne ru pw W akne pv qu Gigs 0 lainc Ajne 0 la jnc kne 0 Lake p qsin tan6 r cos tan 0 qcos rsin w qsin sec0 r cos 6 sec 0 p 0 q 0 r 0 40 Y B 0 4 12 Eq 4 12 defines the dynamic nonlinear function f x t of our state space 4 2 Measurement Sensors The system input includes accelerometers fiber optic rate gyros and high precision GPS measurements The output of a rate gyro is modeled by Ygyro 0 F Baro Mgyro 4 13 where Ygyro is the output of the gyro is the angular rate Byyro is the gyro bias and 7 is zero mean white noise The output of an accelerometer is modeled as Yaccel Une ae RwtoB Jtift Tlaccel 4 14 where Yaccet is the output of the accelerometer ane is non centripetal acceleration a is centripetal acceleration gif is the lift acceleration against gravity 9 8 m s in the 2 direction rotated from the world to body frame and Nacce is zero mean white noise The measurements of position Pn Pe and pg are directly measured by the GPS system as are n Pe and pa the velocity in the world frame Putting these equations together the output of the accelerometers can be modeled using the following Ylaccel Alne qw ru guft sin 0 Yjaccel Ane ru pw gugi cos 0 sin 9 YKaccel akne pv qu gufelcos O cos 4 15
5. AJnc TU pw W akne ak akne pv qu 4 8 Eq 4 8 gives a representation of acceleration in the aircraft s body frame The Euler angle rates can be described using p q and r through the following rotation as shown by 4 1 sindtan cos tan0 p 0 0 cos sin q 4 9 i 0 sindsech cos sec r Finally this rotation takes each of the three angular rates in the body frame and translates them into changes in the Euler angles within the world frame After mul tiplying the following is found p qsin tan 8 r cos tan6 qcos rsin o Y qsin sec 0 r cos o sec 0 4 10 Finding dynamic equations of aine Ajne and akne also referred to as jerk can be difficult Furthermore modeling the gyro biases B Bj and B is also trou 39 blesome For the sake of this problem we assume that Ane 0 1a Q 0 and B 0 4 11 The assumption behind the acceleration model is that if the jerk is non zero the jerk is expected to decrease We also assume that the gyro bias does not change and model as having no change These assumptions have given reasonable results Using Eqs 4 5 4 8 4 10 and 4 11 the dynamics of the aircraft are formed through the following set of state space equations Pn u cOcw v s s cv cosy w c sOcy sosy De u cOsw v sos0sv cosp w c s0sv soey Da u s0 v s c0 w c ct
6. RoR H o H DTR pi 180 REsemi 6378 1363e3 5 Se ajor ax O Kflat 1 298 257 Earth flatness rem N re w e e Re oO Ct m A N RE 1 Kflatx xsin latO0x DTR 2 REsemi local earth radius rel easting RExcos INS 2 DTR sin INS 3 lonO DTR rel northing REx sin INS 2 1lat0 DTR cos INS 2 DTR x 1 cos INS 3 1o0n0 x xDTR x sin latO0xDTR RoR o N y pe o Figure 3 8 A script for converting latitude and longitude to northing and easting in meters variance of a signal is the standard deviation squared these measurements are useful in helping to determine the measurement noise covariance for the Kalman filter 3 5 Conclusion In this chapter I have discussed the need for the MOTRON what the MOT RON is how it was made and what it does I have presented the ways that the MOTRON fulfills the needs of SAR image processing and the ways in which it was designed to make it easy to operate and utilize A guide for operating the MOTRON was also presented as well as sample data Useful data for determining measurement noise covariance was shown and a method for obtaining initial conditions was ex plained Overall the MOTRON has been very successful in meeting the needs of the BYU MERS SAR team and will continue to be useful throughout the foreseeable future 3l Accelerometer Output 0 22 F T T T 0 24 X Ax
7. The MOTRON s operating code found in BYU INU cpp is written in C and contains all of the serial device interfacing NovAtel system communica Figure A 2 The pin numbers of COM1 94 Figure A 3 The pin numbers of COM2 and COM3 Notice that the white circle indicates pin 1 tions and input output handling Serial communications were accomplished via the SerialStream package which is widely available A small set of LCD graphics functions are also defined These make it much simpler to write messages to the LCD screen A set of delays is embedded in each display function to keep the LCD display from displaying erroneous messages Each interfacing device has an assigned Serial Port that can be found at the beginning of the main function Each device must be interfaced within a separate thread so the pthread package was used Each interfacing function operates independently in order to avoid timing issues INU cpp can be easily compiled on board using the command g Iserial pthread o INU INU cpp Once compiled the script executes automatically upon system startup In order to stop the MOTRON from immediately executing this script upon startup edit the TS 7800 s init d script A 4 Additional Resources For more information about the functionality of the TS 7800 visit Technologic Sys tem s online user manual http www embeddedarm com about resource php item 393 95 For additional information reg
8. sOsy 2 5 s0 soch coch where d 0 and y represent roll pitch and yaw respectively and c cos and s0 sind Similarly transforming from the world to body frame requires the same Euler angles but they must be applied in reverse order This is equivalent to multiplying by the transpose of Row chey cO sib s0 Rwir Rhew c si s sci c c s s si s 2 6 sosy cdsdab spcy c s 0ci coch 10 Converting angular rates p q and r into the world frame 6 0 and 1 is also important By recognizing that p db 1 0 0 0 1 0 0 co 0 s0 0 q 0 0 c so 6 0 c s 01 0 0 r 0 0 s co 0 0 s c so 0 c0 i 1 0 s 0 c soc d 2 7 0 s coch q as shown by Beard in 4 and inverting 1 singtan cos tand P O 0 cos sind al 2 8 ab 0 sindsech cos sec r we can express changes in angular rates within the body frame as changes in the Euler angles With these rotation matrices we can represent all of the sensor outputs we receive in terms of one another This is crucial for extracting as much information as possible from the sensors It is very important to understand what coordinate frame the measurements are taken in Position measurements are almost always taken from the world frame via GPS Velocity measurements can be expressed in the world frame north east down velocity or in the body frame velocity in the i j k directions Acceleration
9. 250 300 350 400 450 500 Ss LB ce c LL gt oO 40L 1 f 1 1 3 50 100 150 200 250 300 350 400 450 500 20 T T T T T T T T T 3 D MN A ww e vore Hed i ee wil wiih M Al Ww jen N Arama D O 20 1 1 1 1 1 1 1 1 1 50 100 150 200 250 300 350 400 450 500 Time s Theta deg S 10 5 gt 5 2 D 9 o 5 1 1 1 1 1 1 1 1 1 50 100 150 200 250 300 350 400 450 500 s 10 r 1 r 1 r 1 r r r 3 2 5 J S 50 100 150 200 250 300 350 400 450 500 8 D E Oo 5 1 1 1 1 1 1 1 1 1 50 100 150 200 250 300 350 400 450 500 Time s Psi deg c T T T T T T T T T Q 3 soo as EE UNE o 2 200p 4 ES 3 100 4 o 1 1 1 1 300 350 400 450 500 Ss 1 T T T 3 soo is CN ee P u 200 4 gt 100 4 co o 300 350 400 450 500 a 2 D 2 a 4 1 1 1 1 A 1 1 1 1 50 100 150 200 250 300 350 400 450 500 Time s Figure 5 14 NovAtel solution and the SR UKF solution for roll pitch and yaw 80 Body Frame Velocity m s 7Or 60 40 30 150 200 250 300 350 400 150 200 250 300 350 400 A anaa Aa Mayra O IL el 50 100 150 200 250 300 350 400 450 Time s Non centripetal Acceleration m s 2 T T T T T T T T T o ap n 50 100 150 200 250 300 350 400 450 500 2 T T T T T T T T T gt O 2 1 1 1 1 1 1 1 1 1 50 100 150 200 250 300 350 400 450 500 2 T T T T T T T T T N 2 1 1 1 1 1 1 1 1 1 50 100 150 200 250 300 35
10. 300 350 400 450 500 Time s Theta deg 5 10 3 o 5 72 E zx O gt o 5 S 10 3 e 72 Z O o 5 1 1 1 1 1 1 1 50 100 150 200 250 300 350 400 450 500 45 o S S o ar r gi e AST T a T UI uz a 50 100 150 200 250 300 350 400 450 500 Time s Figure 5 18 NovAtel solution and the low pass filtered SR UKF solution for roll and pitch 83 84 Chapter 6 Conclusion The research provided in this thesis describes a method for estimating flight dynamics from measurements provided by laser gyroscopes accelerometers and high precision GPS integrated with a SR UKF The results of this research are thoroughly tested examined and compared to that of a high cost navigation system and are found to be surprisingly similar Although the results of the SR UKF are less accurate than the NovAtel system they approach the accuracy of the NovAtel system This result is surprising in that the development effort involved and cost of the SR UKF is extremely low compared to the NovAtel system This thesis has also presented a straightforward model for fixed wing aircraft flight dynamics and has presented a simple low cost data recording scheme for high precision motion sensors A SR UKF was implemented using MATLAB software and was optimized for rapid calculation While MATLAB may run much more slowly than C C the MATLAB code gives clear instruction on how to optimally program an SR UKF for future use Due to the nature of
11. 5 13 illustrates the northing easting and altitude measurements of the SR UKF compared to the NovAtel system As the figure shows the position tracking is as accurate as the NovAtel system to within 40 cm This is a better result than in the simulation or on the full set of flight data Also there is a significant improvement in the altitude measurement Because this portion of the flight had very good conditions the position tracking is excellent Table 5 8 shows some statistical differences between the SR UKF solution and NovAtel solution Table 5 8 Difference Between SR UKF and NovAtel Position m Measurement Mean RMS Standard Deviation Max Northing 0 0080 0 2063 0 2061 0 4350 Easting 0 0316 0 1528 0 1495 0 3939 Altitude 0 0032 0 0435 0 0434 0 2534 Figure 5 14 shows the result of the SR UKF versus the NovAtel for this 10 minute flight section The roll difference looks much more white than before and never wanders above 8 degrees Considering that the NovAtel data is smoothed the SR 66 UKF performs very well All three of the Euler angle measurements have a very small difference with respect to the NovAtel solution Table 5 9 contains the statistical data about this difference It is important to note that the SR UKF does not allow the pitch angle to stray very far above 5 degrees This is due to the assumption that the angle of attack is constant At 50 seconds into this flight section we can see that the NovAtel p
12. Cable B Power Switch C ProPak USB Connection D Jump Drive Connection E Ethernet Connection F Serial Terminal Access G Additional Serial Ports for additional measurement instruments Go oy Er au 3 43 ee ee a Xue ed 27 Display screens for operation of the MOTRON 28 A script for converting latitude and longitude to northing and easting IN meters esa ed 31 Stationary accelerometer readings over 70 seconds 32 xxi 3 10 4 1 4 2 4 3 4 4 4 5 4 6 5 1 5 2 5 3 5 4 5 9 5 6 5 7 5 8 5 9 5 10 Stationary gyroscope readings over 70 seconds Body frame versus world frame 7 J and k represent the three orthog onal directions of the aircraft s body frame y and 2 represent the three orthogonal directions in the world frame Time update MATLABsceript nn nn Measurement update MATLAB script Nonlinear function haps MATLAB script Nonlinear function f MATLAB script Nonlinear function hymy MATLAB script The basic Simulink model for the MAGICC Lab flight simulator con sisting of an autopilot block and a flight dynamics block The Simulink model of the UAV flight dynamics yout mat contains the simulated sensor data and xout mat contains the truth data of the flight Lose wok posue qos ne Ee Oe A aye d The flight path of the virtua
13. Kalman filter propagates S which is the Cholesky square root of the matrix Pj Merwe 1 has proposed an efficient method for implementing this modification to the UKF known as the SR UKF The SR UKF makes use of three linear algebra tech niques QR decomposition Cholesky factor updating and efficient least squares 17 First sigma points must be calculated in a slightly different manner Xi Ly Ly n9 Lt n9 gt 2 26 because we are now utilizing S instead of P x is therefore computed the same way as before and the mean is also calculated in the same way The covariance time update must now be performed using the following two steps Sap qr V wf anejo Er VQ i Sij cholupdate Siti Xoti Ser wo 2 27 In the measurement update the equation for P is replaced by the measurement update equation for S Sy qr yw Quarc 941 VR Syy cholupdate Sy Vomi n W 2 28 and the equation for C 1 becomes using MATLAB s efficient least squares algorithm implemented with the operator Ka Palas 2 29 where S sud takes the place of P and 5S is updated by U Kiri Sys Si 11141 cholupdate Sum U 1 2 30 This square root implementation has significant advantages over the UKF According to Merwe 1 the SR UKF is an O L algorithm as opposed to O L for the UKF This is due to the fact that the SR UKF takes advantage of many optimal matrix
14. Kalman filter is a method of recursive and sequential estimation based on a state space model 2 41 Fur En y Hii m 2 12 where F represents the state transition model H is the observation model is the state process noise and y is the observation noise in vector form For simplicity all processes are assumed to be real The process noise is assumed to be zero mean with covariance BEE Qi 2 13 and is assumed to be uncorrelated among samples The observation noise y is also assumed to be zero mean uncorrelated with covariance En Fh 2 14 13 We assume that the process and obervation noise are uncorrelated as well Finally there is some initial condition or priori density on the random variable denoted to 2 4 2 Formulation of the Kalman Filter There are several ways to approach the Kalman filter The method described here is often referred to as the innovations approach The Kalman filter has two important steps the time update and the measurement update The time update is performed using Lite FL Paap FP FT Q 2 15 and the measurement update is performed using Xue Vua xu I ual m Haga Papa I Kia Hi Posie 2 16 where K r Pap HL Hoa Pay Hy RI 2 17 and P is the state covariance matrix and Q and R have been defined previously The Kalman filter is a solution to the general problem of state estimation on a linear system
15. SAR image processing real time calculations are not always necessary However the dynamics estimation system here can be easily im plemented in real time without the low pass filter This is another benefit of the SR UKF compared to the EKF Because the SR UKF runs more computationally efficient than the EKF it is easier to execute in real time while at the same time gaining accuracy The main drawback of the SR UKF described by this thesis is the flight dynam ics assumptions Assuming that sideslip is zero greatly simplifies the SR UKF but also contributes to inaccuracy in the body frame velocity and attitude Furthermore 85 assuming a constant angle to attack makes implementation easier but the results less correct With proper covariance tuning and a lot of patience these assumptions could be done away with Overall the research presented by this thesis has shown that a simple SR UKF implementation can approach the effectiveness of an expensive and complex motion measurement system implemented with an EKF In ideal conditions the SR UKF was nearly as good and cost much less 6 1 Contributions The research presented in this thesis contributes the following A user friendly standalone motion sensor measurement recording system that can be used with multiple sensors of any type MOTRON e A state space flight dynamics estimation model for fixed wing aircraft e An estimation model of three IMAR IMU laser gyroscopes and a
16. Switch C ProPak USB Connection D Jump Drive Connection E Ethernet Connection F Serial Terminal Access G Additional Serial Ports for additional measurement instruments 3 3 2 User Interface Data Storage and Input Algorithms In order to make the user interface as intuitive as possible great care was taken in designing the user input system When the MOTRON is first turned on it displays the baud rate at which the screen is functioning Fig 3 7 a then displays the Data Capture screen shown in Fig 3 7 b 21 CHROMALYTE Inc Ca Us Data Carture 2 8 J 6kB DXIS0 VERI WRITTEN EY TREVOR PAULSE BYU NUSAR EYU LE EVU NUSAR Figure 3 7 Display screens for operation of the MOTRON At this point the MOTRON is ready to go and displays the following screen shown in Fig 3 7 c The MOTRON determines whether or not a flash drive has been inserted If a flash drive has not been inserted correctly it continues to prompt the user to insert a flash drive Once a flash drive has been inserted the MOTRON displays the following screens shown in Figs 3 7 d and e Following this the user is ready to begin collecting data It should be noted that the NovAtel ProPak should be allowed up to two minutes to self calibrate before attempting to record any data Pressing one on the MOTRON keypad begins the recording process and sends a command to the NovAtel ProPak to begin sending 1 Hz time stamps to the SAR system fo
17. algorithms Merwe states that the SR UKF is about 20 faster than the UKF 18 in experimental results With the SR UKF in place we are prepared to apply it to the flight dynamics problem for SAR image processing 2 5 Conclusion This chapter has presented the need for motion measurement a basis for understanding aircraft flight dynamics and a formulation of the Kalman filter With this information a state estimation scheme can be readily initiated 19 20 Chapter 3 Motion Recording Onboard Device MOTRON When the BYU MERS lab purchased a motion sensing system from NovAtel Inc a compact data collection and storage device for this system became necessary As a consequence I created the MOTRON MOTion Recording ONboard device The goal of the MOTRON system is to create a small lightweight user friendly and efficient data collection device Specifically the MOTRON collects data from a Novatel Propak V3 system This system includes an IMU Inertial Measurement Unit consisting of three finely calibrated fiber optic gyros three highly sensitive accelerometers and a high precision GPS system In order to create a motion tracking system a data collection device is needed to accumulate all of the necessary measurements for that system This chapter out lines the requirements that the MOTRON fulfills the specifications of the sensors to be measured the components and operation of the MOTRON and finally examples of data that
18. appreciation to all those who have guided my education I would especially like to thank my advisor friend and mentor Dr David Long He has helped me not only with my research but with many important deci sions I have faced aLong the way Special thanks go to Dr Randal Beard for taking time out of his busy schedule to help teach me Kalman filtering basics I would also like to express my appreciation towards my colleagues Bryce Ready and Matthew Tolman who spent more time than they probably could afford working with me and all of my friends in the MERS lab Finally I would like to thank my family for always being encouraging and supportive Particularly I thank my wonderful wife Caryn who has been so patient with me through this whole process Table of Contents Acknowledgements xiii List of Tables xix List of Figures xxiii 1 Introduction 1 Ll Purpose 244464684 Ros e ORG be 244 RAO EG Rd URS d 1 1 2 Contributions 24 sa suyas RE oe eee EA 2 IA or aa 2 ee O eS Ae eS 2 2 Background 5 21 iso PD 5 2 2 SAR Image Processing 1 ss cs ses sus and 5 2 2 1 SAR System Overview nn 5 2 2 2 The Need for Motion Compensation 6 2 3 Aircraft Flight Dynamics ious k Voce oe BERRY o n RR a eed 8 2 3 1 Coordinate Frames 2 vun 22er OE OE eee ee ee 8 2 3 2 Characteristics of Aircraft Motion 12 2 4 The Kalman Filter 45022 882 2 ra a 13 2 4 1 The State Space Model e 13 24 2 Fo
19. can be represented in further complicated frames This is due to the fact that every time we differentiate a measurement it must be differentiated with respect to a coordinate frame For example differentiating body frame velocity with respect to the world frame gives non centripetal acceleration Differentiating the body frame velocity with respect to the body frame gives an acceleration measurement that includes centripetal acceleration In addition angular rates measured by gyroscopes are measured in the body frame while Euler angles are measured in the world frame 11 2 3 2 Characteristics of Aircraft Motion With an understanding of coordinate frames a brief discussion of aircraft flight characteristics is warranted First one of the most important characteristics of an airplane is understanding how it turns When an aircraft turns it rolls to accomplish a change in heading This is known as a coordinated turn Note that if the roll angle is zero the change in world frame heading is related only to the wind and sideslip angle If sideslip and wind are neglected then the roll angle is zero if there is no change in heading Under these conditions a coordinated turn can be modeled by p I tan Q 2 9 U where 1 is the change in heading g is the acceleration due to gravity amp is the roll angle and v is the total velocity of the aircraft Furthermore we can model the body frame velocity according to u vcosa cos D
20. v vsin p w vsin a cos f 2 10 where u v and w are the body frame velocity in the 7 7 and k directions respectively v is the total velocity o represents the angle of attack and P represents the sideslip angle Neglecting sideslip also allows us to assume that all of the velocity present in the body frame of the aircraft is present in the and k directions This means that u V COS Q v 0 w vsina 2 11 Assuming sideslip is zero is an approximation but for larger aircraft with a large rudder it can be a good approximation Assuming no wind is a worse approxi 12 mation but if we assume that SAR readings are made on sufficiently calm days then it can be acceptable These assumptions greatly simplify the dynamics and help us to formulate a simpler state space model for the Kalman filter 2 4 The Kalman Filter The Kalman filter is a recursive estimator used to estimate the state of a linear time varying state equation in which the states are driven by noise and observations are made in the presence of noise There are many variations of the Kalman filter particularly when extending this idea to non linear models This section addresses the basics of Kalman filtering and useful extensions of the idea using the Unscented Kalman Filter UKF and it s square root variation for numerical stability The notation used in this section for the Kalman filter is that of Moon 5 2 4 1 The State Space Model The
21. 0 100 150 200 250 300 350 400 450 500 T T T T T T 10r S o I Lu 10 1 1 1 1 1 1 1 1 1 1 50 100 150 200 250 300 350 400 450 500 Time s Theta deg T T T T T T T T T T 20r 4 E 2 10F Hi O 1 1 1 1 1 1 50 100 150 200 250 300 350 400 450 500 5 r r T r r r T T r r 5 20r lt u 10 7 5 50 100 150 200 250 300 350 400 450 500 10r 5 uw of 10 1 1 1 1 1 1 1 1 1 50 100 150 200 250 300 350 400 450 500 Time s Psi deg T T T T T T T 1 T 300 E 4 E cs 5 200 E 100 N 4 O 1 1 S r r r r r r r r r r 3 300 N N co L u 200 5 100r 4 1 cc OE 1 1 i A us 50 100 150 200 250 300 350 400 450 500 5 L 4 S o ui 5L 4 1 1 1 1 1 1 1 1 1 50 100 150 200 250 300 350 400 450 500 Time s Figure 5 7 Truth data and the SR UKF solutions for roll pitch and yaw See text and Table 5 4 for analysis 13 p tania S o ef T L L E 100 150 20 350 40 500 S i 3 eat o IS ic d e 50 150 20 350 40 500 0 01 i 5 amp o bl p m T HH ob q lia 0 01 i 150 20 250 500 ae q rad s 2 l f l i h l s nm moe ad 3 p o 2 4 Ar 50 100 150 200 250 300 350 400 450 500 s 2 i i i i i i i i 3 of I forth th
22. 0 400 450 500 2 T T T T T T T T T T Ss d O 2 1 1 1 1 1 1 1 1 1 1 50 100 150 200 250 300 350 400 450 500 Time s Altitude m 52 T T T T T T T T T T m A 3 E 2 3 o c LL gt cc c 5 uw 1 1 1 1 1 1 1 1 1 50 100 150 200 250 300 350 400 450 500 Time s Figure 5 4 Truth data SR UKF solutions and SR UKF error for northing easting and altitude See text and Table 5 1 for analysis 70 Truth SR UKF Solution e o Lu 4 1 1 1 1 1 1 50 100 150 200 250 300 350 400 450 500 Time s v m s T r T T r T T T r r 4 5 3 0 E 4 m 1 1 50 100 150 200 250 300 350 400 450 500 5 T T 3 a LE zi 5 1 1 1 1 1 1 50 100 150 200 250 300 350 400 450 500 5 uw 1 1 1 1 1 1 1 50 100 150 200 250 300 350 400 450 500 Time s w m s 4 T T T T T T T T T T E 22 E o A A f 1 f A 50 100 150 200 250 300 350 400 450 500 s 4 r r r r r r r r r B 55 o miaka alandada laalaa ahahha kiia gt 5 o 1 50 100 150 200 250 300 350 400 450 500 2 r r r r r r r r r 17 e io O 2 L L 1 L L L 1 L L 50 100 150 200 250 300 350 400 450 500 Time s Figure 5 5 Truth data and the SR UKF solutions for u v and w See text and Table 5 2 for analysis 71 Non centripetal Acceleration in the x Direction m s Truth
23. 0 400 450 500 Time s Angular Rates rad s 0 2 T T T T T T T T T 0 2 f 50 100 150 200 250 300 350 400 450 0 2 T r 0 2 1 1 1 50 100 150 200 250 300 350 400 450 500 0 2 T r r r r r r r r o Ae At perba un M pi A yet d 0 2 1 1 1 1 1 1 1 50 100 150 200 250 300 350 400 450 500 Time s Figure 5 15 NovAtel solution and the SR UKF solution for the body frame velocity non centripetal acceleration and angular rates 8l Frequency Content of Phi 150 T T T T 100 Magnitude dB a o o 50 i i i O 0 2 0 4 0 6 0 8 Normalized Frequency pi rad sample 0 2 0 4 0 6 0 8 Normalized Frequency pi rad sample a Figure 5 16 Frequency content of Notice the strong harmonic content at 0 217 0 427 0 637 and 0 847 rad sample Frequency Response of the Low Pass Filter 50 r r T r ea o 4 D S S 501H um c 100 150 i i i i O 0 2 0 4 0 6 0 8 1 Normalized Frequency pi rad sample Oo T T T T g 100 E 200 8 ol c 300 400 i i i i O 0 2 0 4 0 6 0 8 1 Normalized Frequency pi rad sample Figure 5 17 Frequency response of the low pass filter 82 Phi deg Q 5 e ce 2 lt gt o lt 2 5 e ce Lu gt a ceo 5 ce 2 e a 10 50 100 150 200 250
24. 41 The gyros can be modeled as Ylgyro p By Yjgyro q By Ykgyro r Bk 4 16 and the GPS position can be modeled Northing Pnr Easting pe Altitude pag 4 17 and lastly the world frame velocity is given by Dn u cOcw v s s0ci cosy w co s0cv sosy De u c sv v s s0sv cosy w c s0sv soci Pa u s0 v sQc0 w c c0 cos0cv sosy 4 18 Each of these equations define an output function for the Kalman filter 4 3 Implementing a Square root Unscented Kalman Filter With these set of equations we can define f t and h x t the nonlinear functions of a state space model General implementation of the square root un scented Kalman Filter SR UKF is outlined in Chapter 2 This section discusses a particular implementation of the SR UKF in order to solve the state space model Tili 1 iaa E 4 19 Y h Lii 1 t 9 4 20 42 where and n represent process and measurement noise respectively The SR UKF uses a set of scalar weights Wr 2 A L X Wi A L4 4 1 o 8 Wr WE 1 B L A 4 21 where L a 1 4 22 is a scaling parameter Values of a 1 5 6 2 and L 18 are used throughout the SR UKF implementation o determines the spacing of the sigma points 4 should be equal to two for Gaussian noise and L represents the number of states in the system 4 3 1 Finding Initial Conditions In order to initi
25. 77 0 5073 0 5066 1 0130 Easting 0 0042 0 3633 0 3633 1 1945 Altitude 0 0035 0 0618 0 0617 1 2412 Figure 5 5 illustrates the body frame velocity output of the filter As in Fig 5 4 the figure is divided into three parts showing the u v and w SR UKF measurements with respect to the truth The body frame velocity is one of the most difficult states to track accurately due to the lack of any direct measurement of speed in the body frame Neglecting sideslip one of the assumptions previously made is clearly not the best decision here Ideally v is always zero in the absence of sideslip but we can see from Fig 5 5 that v is clearly not always close to zero throughout the flight This assumption negatively affects the output of the SR UKF however with proper Q tuning these errors can be somewhat reconciled as is shown in the output of the SR UKF The other assumption that u and w follow a constant angle of attack is a better assumption but still not ideal Due to the UAV s behavior the body frame velocity is under constant change shown by the large spikes in u and w While these 60 spikes are certainly difficult to track they are not large enough to cause significant problems for the SR UKF The error statistics are given in Table 5 2 The mean error in w is caused by the virtual measurement of the angle of attack in the Kalman filter While this virtual measurement may cause a small mean error in w it is beneficial overall to
26. 9 CPU 128 MB DDR RAM 512 MB of NAND Flash memory 2 USB ports a gigabit Ethernet connection 10 serial ports and boots Linux Debian from the onboard Flash mem ory The user interface consists of a small serial graphic LCD and twelve button serial keypad The LCD screen is a blue white 128 x 64 graphic display made by MicroCon troller Pros Corporation It is controlled through simple ASCII commands through a standard RS232 protocol The keypad interface is a basic 3 x 4 keypad array made by Storm Interface Connected to the keypad is a keypad switch that converts the keypad entries into ASCII symbols and transmits them over a serial RS232 protocol Since the display is input only and the keypad is output only I saved a serial port by setting the LCD and keypad to operate at the same baud rate and use the same serial port Each of these components are put together inside of a grated metal box shown in Fig 3 5 The grating allows proper airflow for cooling Each of the necessary connections to the computer are also brought to the outside of the box using the standard connections shown in Fig 3 6 The MOTRON is mounted to a metal plate with four bolts which makes it easy to attach inside an aircraft This design is intuitive and easy to use Each of the cord sockets are clearly labeled in order to avoid confusion and the user interface is designed to be clear and understandable 26 Figure 3 6 MOTRON connections A Power Cable B Power
27. B code the operator is used in place of The MATLAB script can be seen in Fig 4 5 In this particular implementation we receive measurements in two different packets We receive the IMU measurements accelerometers and gyros at a rate of 200 Hz and we receive GPS measurements position and ground speed at a rate of 47 function new_xhat new_Si measureupdate xhat Si 2 number_of_measurements 3 4 M number_of_measurements 5 sigma_points xhat xhat ones 1 L etaxSl 6 xhat ones 1 L etaxSi 7 y points h sigma_points 8 yhat y points 1 9 Junk Syi gr Wcix y points 2 end 10 yhat ones 1 M 1 chol R 11 if Wcl gt 0 12 Syi cholupdate Syi 1 M Wcl y points 1 13 yhat 14 else 15 Syi cholupdate Syi 1 M Wcl y points 1 16 yhat 17 end 18 Syi Syi 19 Pxy Wclx sigma points 1 xhat y points 1 yhat 20 for 3 2 Lx2 1 21 Pxy Pxy Wcix sigma_points j xhat x 22 y points j yhat 23 end 24 K Pxy Syi Syi 25 new xhat xhat Kx measurements yhat 26 U KxSyl 27 for j 1 M 28 Sip p cholupdate Si U 3 29 if p NOJA 30 fprintf Si negative definite 31 else 32 new_Si Sip 33 end 34 end Figure 4 3 Measurement update MATLAB script 50 Hz This necessitates the use of two separate h functions T
28. Fulton College of Engineering and Technology ABSTRACT LOW COST HIGH PRECISION FLIGHT DYNAMICS ESTIMATION USING THE SQUARE ROOT UNSCENTED KALMAN FILTER Trevor H Paulsen Department of Electrical and Computer Engineering Master of Science For over a decade Brigham Young University s Microwave Earth Remote Sens ing MERS team has been developing SAR systems and SAR processing algorithms In order to create the most accurate image reconstruction algorithms detailed aircraft motion data is essential In 2008 the MERS team purchased a costly inertial mea surement unit IMU coupled with high precision global positioning system GPS from NovAtel Inc In order to lower the cost of obtaining detailed motion measure ments the MERS group decided to build a system that mimics the capability the NovAtel system as closely as possible for a much lower cost As a first step the same sensors and a simplified set of flight dynamics are used This thesis presents a standalone motion sensor recording system MOTRON and outlines a method of utilizing the square root Unscented Kalman filter SR UKF 1 to estimate aircraft flight dynamics based on recorded flight data as an alternative to the extended Kalman filter While the results of the SR UKF are not as precise as the NovAtel results they approach the accuracy of the NovAtel system despite the simplified dynamics model ACKNOWLEDGMENTS I would like to express my gratitude and
29. LOW COST HIGH PRECISION FLIGHT DYNAMICS ESTIMATION USING THE SQUARE ROOT UNSCENTED KALMAN FILTER by Trevor H Paulsen A thesis submitted to the faculty of Brigham Young University in partial fulfillment of the requirements for the degree of Master of Science Department of Electrical and Computer Engineering Brigham Young University April 2010 Copyright 2009 Trevor H Paulsen All Rights Reserved BRIGHAM YOUNG UNIVERSITY GRADUATE COMMITTEE APPROVAL of a thesis submitted by Trevor H Paulsen This thesis has been read by each member of the following graduate committee and by majority vote has been found to be satisfactory Date David G Long Chair Date Richard W Christiansen Date Doran K Wilde BRIGHAM YOUNG UNIVERSITY As chair of the candidate s graduate committee I have read the thesis of Trevor H Paulsen in its final form and have found that 1 its format citations and biblio graphical style are consistent and acceptable and fulfill university and department style requirements 2 its illustrative materials including figures tables and charts are in place and 3 the final manuscript is satisfactory to the graduate committee and is ready for submission to the university library Date David G Long Chair Graduate Committee Accepted for the Department Michael J Wirthlin Graduate Coordinator Accepted for the College Alan R Parkinson Dean Ira A
30. When using a Kalman filter on linear Gaussian systems it is an optimal estimator This is due to the fact that Gaussian distributions are completely described by a mean and covariance Unfortunately this derivation of the Kalman filter is only useful for linear systems To be used with aircraft dynamics a different approach to the Kalman filter must be taken 14 2 4 3 Non Linear Extensions of the Kalman Filter Consider a general nonlinear system of the form Ly Fed Bu y h z t 2 18 where f and h are nonlinear functions of a and t The general nonlinear estimation problem is extremely difficult and no gen eral solution to the general nonlinear filter problem is available One reason the linear problem is more convenient to solve is that when the noise and initial conditions are Gaussian the state E is guaranteed to be Gaussian as well However if f or h is nonlinear then is not guaranteed to be Guassian so either a linearization is re quired or there must be a different approach to estimating the state error covariance A typical approach to solving this problem is to linearize the nonlinear function f using the Jacobian Ox Ox2 Ox Ofo Of Of Of on e 7 er 2 19 dr 0x1 0x2 Ox where L is the dimension of the state With this approach we can then use the Jacobian of f in place of the matrix F of the linear Kalman filter but updated at every sample Similarly a linearization of h
31. a set of simulated sensor measurements the Kalman filter is applied to the recorded data The following set of graphs illustrate the truth data the output of the Kalman filter and the Kalman filter error for each of the states in the filter Figure 5 4 shows the position measurements of the SR UKF The figure is divided into three groups of three graphs northing easting and altitude The first of 59 the three graphs shows the truth data the second shows the SR UKF solution and the third is the error all of which are represented in meters It is clear that the SR UKF and truth data follow a very similar trend The SR UKF solution is generally less than 1 meter away from the actual position for each of the three position measurements Table 5 1 gives some important statistics about the SR UKF error The mean error is close to zero and the maximum error is close to 1 meter By observing the altitude data it is easy to see that the small UAV in this flight jumps and dips considerably dropping 2 meters within 2 seconds which significantly effects the accuracy of the SR UKF This is a rather extreme flight pattern that would not normally occur on the MERS aircraft nonetheless the SR UKF does a reasonable job of tracking the position despite the somewhat erratic simulated aircraft platform behavior Table 5 1 Northing Easting and Altitude Errors m Measurement Mean Error RMS Error Standard Deviation Max Error Northing 0 02
32. aft Unfortunately the MERS aircraft is considerably different from the aircraft modeled by this flight simulator The UAV is considerably lighter smaller and slower than the MERS aircraft This means that the angle of attack aerodynamic coefficients turning rate rate of climb inertial moments lift and drag of the UAV are very different from the MERS aircraft Sideslip characteristics are very different because the UAV is much lighter and moves more slowly while not having a rudder Because the MES aircraft is larger and moves more quickly with a rudder it does not suffer from as much sideslip and has a lower angle of attack These differences certainly affect the SR UKF s performance but the simulator is still useful to provide an assessment of the functionality of the SR UKF 58 Virtual UAV Flight Path 300 T T T T T T T T 200 100 E 0 Oo amp 3 100 200 300 400 l 250 200 150 100 50 0 50 100 150 200 Easting m Figure 5 3 The flight path of the virtual UAV The UAV starts at the center and flies a clockwise path around four different waypoints several times 5 2 Kalman Filtering the Simulated Data Once the simulator has been initiated it directs the virtual aircraft to fly the path shown in Fig 5 3 As the UAV flies the computer generated data and observation time are recorded After running the flight simulator for a few minutes allowing the UAV to fly several rounds and collecting
33. alize the Kalman filter the MOTRON s data set must provide initial conditions for each of the states in the Kalman filter In order to simplify this process the IMU is allowed to sit still for a few minutes while initial conditions are calibrated By allowing a calibration time we can assume the initial position is the mean position of that period The initial velocity is set to zero and the initial attitude is calculated in a straightforward method The lift force against gravity is assumed to be 9 8 m s in the 2 direction Using the rotation matrix Rwtog from Eq 4 2 we can determine the initial roll and pitch Ax 0 Ay RwtoB 0 4 23 Az 9 8 43 giving Ax sin0 9 8 Ay sin cos 0 9 8 Az cos cos 0 9 8 4 24 which leads to 00 sin 4 25 o sin E 4 25 A ERIT Z 4 26 E 9 8 cos 0 626 where A represents the mean value of the acceleration over the calibration time 4 represents the initial pitch and represents the initial roll After calculating the initial pitch it is plugged into Eq 4 26 Finding the initial heading is done in a similar manner to Eqs 4 25 and 4 26 but using the gyros Gx Q cos C Gy Rwio 0 4 27 Gz Q sin C G represents the average gyroscope reading during the calibration period represents the rotation of the Earth 7 2722 x 10 rad s and represents the average latitude over the calibration period This equation y
34. apter 4 First the Kalman filter is applied to a simulated data set that contains absolute truth data Then the Kalman filter is applied to actual recorded data and an explanation of the results is provided 5 1 Flight Simulator In order to gauge the effectiveness of the SR UKF I first use simulated sensor and position data from a flight simulator The flight simulator provides computer generated accelerometer gyroscope and GPS readings and also provides absolute truth data about the position velocity and attitude By applying the SR UKF to the computer generated sensor data and comparing its output to the truth it is easy to judge the effectiveness of the SR UKF A flight simulator created by BYU s MAGICC Lab is used to create a set of synthetic data 7 The simulator is typically used to model small unmanned aerial vehicles Modifications have been made to more closely simulate the sensors that are used onboard the MERS aircraft It is constructed in MATLAB s Simulink and consists of an autopilot block and a flight dynamics block shown in Fig 5 1 The autopilot system is outside the scope of this thesis however the flight dynamics block is important to the results and is shown in Fig 5 2 95 UAV Simulation BYU MAGICC Lab Waypoints Autopilot UAM Figure 5 1 The basic Simulink model for the MAGICC Lab flight simulator consisting of an autopilot block and a flight dynamics block As is shown in Fig 5 2 the out
35. ar that the SR UKF and NovAtel systems share 64 common trends Table 5 7 gives data about the error between the NovAtel and SR UKF systems Several assumptions about the aircraft s flight account for some of the difference The assumption of constant angle of attack does not allow the SR UKF solution to grow in pitch very much because the virtual measurement of the body frame velocity particularly u and w pull it down This assumption does not allow the SR UKF to achieve the higher angles of pitch that may have existed in the actual flight and that the NovAtel system may have reported These discrepancies can be found between times 0 200 and 400 600 Furthermore the assumption of zero sideslip contributes to some of the error in the roll angle Because sideslip is not likely zero in reality whenever there exists non zero velocity in the v direction there is an error in the roll angle This is evident at times 100 300 1000 and 1300 Also because there is no direct measurement of the body frame velocity or attitude and because the NovAtel implements a smoother the SR UKF solution is significantly noisier than the NovAtel solution Because of these factors the greatest difference is found in the roll angle Table 5 7 Difference Between SR UKF and NovAtel Attitude Measurement Mean RMS Standard Deviation Max Roll 0 1250 3 1797 3 1773 16 4655 O Pitch 0 1841 0 9416 0 9235 3 6347 Yaw 0 3515 0 4980 0 3527 1 5411 F
36. arding the LCD screen visit http microcontrollershop com product_info php products_id 1974 96
37. at the attitude position and velocity data are as accurate as possible For this reason the input sensors are some of the best sensors available By further processing this sensor data using a Kalman filter the accuracy and utility of the data is improved The following chapter outlines a method of estimating attitude and position data using the square root unscented Kalman filter presented in Chapter 2 Section 4 1 summarizes the dynamics of aircraft flight and propose a set of state variables for modeling these dynamics Section 4 2 defines a model for the output sensors of the system Finally Section 4 3 presents the details of setting up an unscented Kalman filter to implement the state space model to be presented 4 1 Aircraft Flight Dynamics and State Variables Prior to developing the filter it is important to first look at the dynamics of the system to be modeled By understanding the underlying principles of aircraft flight we can more accurately model it The expressions presented are general to any rigid body and are derived from equations presented in Chapter 2 35 4 1 1 State Variables The state variables of the aircraft are the following eighteen quantities Pn Northing position of plane De Easting position of plane Pa Down position of plane negative altitude u Body frame velocity in the 7 direction v Body frame velocity in the 7 direction w Body frame velocity in the k direction Gin Non centr
38. bility to penetrate clouds independence of the sun as a source of illumination and the ease at which man made objects can be rec ognized Often the combined use of SAR and photography allow a greater study of geometric and molecular properties of a surface SAR systems are very complex devices and as such only a brief overview of SAR systems is given here 2 2 1 SAR System Overview In a typical SAR application the SAR antenna is attached to the side of an aircraft Once the aircraft has achieved level flight a pulse from the antenna is trans mitted to the ground below Because diffraction requires a large antenna to produce a narrow beam a single pulse from the radar is rather broad often illuminating the terrain from directly beneath the aircraft to the horizon If the topography is approx imately flat the times at which that echo returns allow objects at different distances flight path m rango A Figure 2 1 SAR uses multiple antenna pulses to create a synthetic antenna By antenna footprint combining each pulse return from an area high resolution images can be created to be distinguished If the amplitude and phase of the return signal from a piece of ground are recorded and the aircraft emits a series of these pulses as it travels the results of these pulses can be combined to make a much higher resolution image In effect the series of observations can be joined together just as if they had all been made conc
39. ccelerometers A unique formulation of the square root Unscented Kalman filter in which the mean value is not a traditional weighted sum but simply the previous state mean passed through the nonlinear function f e A method for estimating initial attitude conditions of a stationary aircraft A method of integrating simulated measurements into actual flight data to improve filter accuracy A functional flight simulator that correctly models the NovAtel sensors onboard a small UAV e A fully functional flight dynamics estimation system 6 2 Future Work Areas in which future work may be done include 86 e Elimination of flight dynamics approximations By successfully eliminating the angle of attack sideslip coordinated turn and acceleration assumptions the estimation scheme can be applied to any free moving body In other words the estimation system can be used in automobiles watercraft helicopters as well as fixed wing aircraft It would also make the Q tuning independent of the platform which would be very useful for not only SAR but robotics and guidance systems e Use of quaternions Using quaternions would eliminate the need for Euler an gles and hence eliminate the problems associated with their use Quaternions remove the chance of gimbal lock which occurs when two of the three axes of rotation in a three dimensional space are driven to the same direction resulting in the loss of one degree of freedom By usi
40. cess to the MOTRON s command line in order to modify its operation The first method is connecting serially through the Terminal ttyS0 RS232 port at 115200 baud using the username root and password hello The second method is to ssh through the network connection with the command ssh root 192 168 0 50 using the password hello Once a connection has been made the user can change the MOTRON interface by editing the file INS cpp located in the ABYUA Interface directory Recompiling INS cpp is done with the command g lserial pthread o INS INS cpp Once compiled the program runs automatically upon bootup via an etc init d script 3 3 3 Interpreting the Recorded NovAtel Data The GPS and IMU packets arrive in double precision binary format From the GPS packets latitude longitude roll pitch and heading are given in degrees 29 Table 3 1 IMU Scale Factors Sensor Scale Factor T Gyroscope 1 65888x107 rad s Accelerometer 5752 1 6384x 101 and altitude is given in meters North east and up velocity are given in m s From the IMU packets accelerometer readings are given by the change in velocity count along a certain axis Similarly the gyro measurements are given in change in angle count This means that the output of the IMU packet RAW txt must be scaled by a constant after it has been recorded in order to make the data meaningful The scale factors used t
41. d frame Y 2 through the following rotation matrix cab cosy 4 sos0 c sosy cbsdc b Rpuw Os cocw sdhsbsy soci c sOs 4 2 s0 soch coch 37 This matrix takes vectors within the body frame j k and rotates them into the world frame 2 y 2 In order to move from the world frame to the body frame we simply multiply by Rwiog which is the transpose of Row Rwiop Hou 4 3 4 1 3 Equations of Motion In order to calculate the northing easting and down velocities pn Pe Pa we rotate the body frame velocity u v w through Rg ow This gives us the velocity in the world frame pn Pe and pa Dn u Pe Reow v 4 4 Pa w yielding Dn u cOcy v s s0ci cosy w c s0cy sosy Pe u c sw v s s0sv cosy w co s0sv soep Pa u s0 v s c0 w c c0 4 5 The body frame acceleration w can be modeled as the sum of the non centripetal acceleration and centripetal acceleration in the body frame Centripetal acceleration is found by taking the cross product of the angular rates with the body frame velocity This is known as the Coriolis equation Ale p u aje m q x Y 4 6 ak T w 38 and yields Qi QU rv aJe YU pw ak pv qu 4 7 Eq 4 7 gives an expression for centripetal acceleration which can be used to find the total body frame acceleration aine Ale Aine qw rv afne Ae
42. e CPU to run at 333MHz rather than 500MHz to save power and allow it to run in hotter thermal conditions To turn on the TS 7800 connect a regulated 5VDC to the pins shown in the bottom left of Fig A 1 All of the boot messages are displayed through COM1 by default Once started the TS 7800 boots Debian Linux from the on board flash memory if an SD Card is not inserted The board is set to bypass a fast bootup sequence and proceed directly to a full operating system bootup To get back to the fastboot shell touch the file fastboot in the root directory of the Debian filesystem To turn it off again issue the command In sf linuxrc mtdroot linuxrc save Finally in the case of a crash to reprogram the entire TS 7800 flash memory boot from a backup SD Card and issue the command createmtdroot from the shell This will restore all necessary files to operate the MOTRON The MOTRON can also be operated completely from a backup SD Card Once booted the TS 7800 runs a full featured Debian Linux distribution It includes everything necessary to run Linux and develop Linux applications The TS 7800 also includes an on board C C compiler for developing custom applications To compile a source file just issue the command g o outfile sourcefile cpp All of the MOTRON s source code is in the BYU directory including INU cpp which contains the code for the MOTRON data recording program A 2 COM Connection
43. e UAV flight dynamics yout mat contains the simulated sensor data and xout mat contains the truth data of the flight ates modeled sensor data The gyroscopes are modeled using gyro p Ogyrof gyro 4 Ogyro6 gyro r Ogyro6 where p q and r are the angular rates coming from the true UAV angular rates in xout mat is the gyroscope noise variance parameter set in param m and is a zero mean normally distributed random number The accelerometers are modeled by accel Fy F Fihrottle F O accel accel Fy alt accel F Oaccel 57 where F Fy and F are the aerodynamic forces acting upon the aircraft calculated using the properties of the aircraft defined in param m and Fihrottle is the force applied by the throttle Finally the GPS is modeled by pnaps Pn OapsE peaps Pe Oaps pdaps Pa ears Nyel pnaps PNprev cps T serps Cvel pears E Dprev GPS T saps dva pdaps pdprev aps T saps Yaps Y Caps where Pn Pe and pa are the true position and w is the true heading The GPS velocities are calculated from noisy positions so no extra noise is added They also use the GPS sample time to find the measured velocity In each of the previous equations noise characteristics similar to those of real measurements are used These simulated measurements are chosen to most closely model the sensors that are used on the MERS aircr
44. e antenna or GPS satellite elevation placement and installation of the antenna is easy It connects to the ProPak using a standard coaxial cable shown in Fig 3 2 and is permanently attached to the aircraft The third component of the NovAtel system is the Inertial Measurement Unit IMU The particular IMU that came with the system is an IMAR iIMU FSAS tactical grade IMU The iIMU FSAS is a relatively small size IMU consisting of three fiber optic rate gyros of class 0 75 deg hr and three servo accelerometers of class 1 mg This unit is pictured in Fig 3 4 It is designed for ruggedized applications and is internally equipped with shock absorbers It communicates with ProPak via RS422 on an HDLC protocol After each component has been connected properly the data is transmitted across the USB serial port Two packets of information are sent to the MOTRON at different rates The GPS solution arrives at 50 Hz and includes GPS position velocity 23 Figure 3 2 Connections of the NovAtel A Power Socket B To MOTRON C To SAR System D amp E To IMU F To Antenna and ground track heading with their associated time stamps The IMU reading arrives at 200 Hz and includes the output of the rate gyros and accelerometers with their associated time stamps 3 2 2 MicroStrain 3DM GX1 and Additional Functionality While the MOTRON primarily interfaces with the NovAtel ProPak the MOT RON is designed to be able to interface with multiple devices T
45. e outside sensors such as GPS or RADAR measure information in the world frame Because all of this information needs to be incorpo rated it is necessary to be able to express any measurement in terms of any other coordinate system One coordinate frame is transformed into another through two basic opera tions rotation and translation Rotations can be performed using a rotation matrix A rotation matrix is an n dimensional square matrix that acts as a rotation of Eu clidean space Two aircraft coordinate frames the world frame and the body frame are related to each other through a rotation When rotated through three Euler angles roll pitch and yaw it is possible to rotate from the body frame to the word frame and vice versa However a single transformation through these three angles is not always unique In order to create a unique transformation it is necessary to restrict the order of these Euler rotations Figure 2 4 The three Euler angles is roll 0 is pitch and v is yaw Each follow the right hand rule about the body frame coordinate axis Therefore to reach the world frame from the body frame we construct a rotation matrix by first rolling then pitching then yawing This is equivalent to rotating three times or multiplying three rotation matrices cw sypy 0 c0 0 s0 1 0 0 Rgww sb cy 0 0 1 0 0 c s 0 0 1 s0 0 c0 0 s c dab cobsy sosO0cy sosy cosOcy c s c c s s0si s dcy c
46. e velocity non centripetal acceleration and angular rates See text for analysis 78 x 10 Northing m ovAtel Solution 2 0 py 50 100 150 200 250 300 350 400 450 500 SR UKF Solution 0 ap 50 100 150 200 250 300 350 400 450 500 0 5 T T T T T T T T T g D 5 Oof 7 x a 0 5 i l 50 100 150 200 250 300 350 400 450 500 Time s Easting m 5000 T T T T T T T T T 5000 ovAtel Solution o 10000 5000 T r r T r r r T r SR UKF Solution o 5000 10000 i i j i i i 50 100 150 200 250 300 350 400 450 500 0 5 3 S gt o a 0 5 1 i 50 100 150 200 250 300 350 400 450 500 Time s Altitude m 5 T T T T T T T T T 1600 4 o c B 1400 4 5 2 1200 i i i i i i i 50 100 150 200 250 300 350 400 450 500 e Ss 3 1600 4 c DZ 1400 J T 1200 i j i j i i i j 50 100 150 200 250 300 350 400 450 500 0 5 i i 1 f i 3 S BO oye Hin lst HO i a 50 100 150 200 250 300 350 400 450 500 Time s Figure 5 13 NovAtel solution and the SR UKF solution for northing easting and altitude 79 Phi deg AOF T T T T T 3 20r 4 c ob Zs 20 f 2 40L 1 1 1 1 1 i 1 1 j 50 100 150 200
47. ect data from laser gyroscopes accelerometers and high precision GPS It introduces the requirements the MOTRON must fulfill and how the MOTRON meets those requirements e Chapter 4 outlines the specific implementation of the SR UKF to the MERS aircraft flight dynamics estimation problem This chapter also gives specific approximations that are used by the SR UKF to simplify calculations e Chapter 5 presents the results of the SR UKF on simulated data and on actual recorded data In the simulated case the output of the SR UKF is compared to the truth data provided by the simulator and in the actual flight the SR UKF solution is compared to the NovAtel solution e Chapter 6 concludes the thesis and suggests areas in which further work can be done to refine the SR UKF implementation Chapter 2 Background 2 1 Introduction In order to create a motion tracking system it is important to look at why such a system is needed This chapter discusses the need for a motion tracking system reviews basic aircraft dynamics and provides a formulation of a nonlinear square root unscented Kalman filter This chapter also outlines assumptions made in flight dynamics in order to simplify state space calculations 2 2 SAR Image Processing Microwave remote sensing particularly Synthetic Aperture Radar SAR has been in use since the 1960s Some of the significant advantages of SAR over con ventional aerial photography are its a
48. ective and is shown in Fig 5 17 An equiripple FIR filter of order 1126 is chosen using the Parks McClellan method to lower the noise and all the harmonic content A very long filter is needed in order to isolate the low frequency content of interest 0 001 to 0 005 rrad sample while keeping the side lobes down to a low level Although this filter is long it is less than 1 100 of the size of the roll estimate However a filter of this length would be unreasonable to use in real time applications Fig 5 18 shows the resulting attitude estimates after applying the low pass filter As before the NovAtel solution is given first followed by the filtered SR UKF solution and their difference for roll and pitch respectively No filtering is applied to the yaw angle due to the irregular 0 360 wrapping Table 5 10 gives the new error statistics after filtering As can be seen in the figure and table the error is greatly reduced by the low pass filter The maximum error has been reduced by half the mean error is much closer to zero and the graph looks much more similar to the NovAtel solution Table 5 10 Difference Between Filtered SR UKF and NovAtel Attitude Measurement Mean RMS Standard Deviation Max Roll 0 0946 1 1863 1 1825 5 0120 0 Pitch 0 4372 0 8622 0 7431 1 7848 w Yaw 0 3484 0 5591 0 4372 1 5505 5 5 Conclusion Overall the SR UKF does a reasonable job estimating the simulated UAV movement and performs very well
49. el solution and the SR UKF solution for northing easting and al DE 33623 9029 a oe due we a ee NovAtel solution and the SR UKF solution for roll pitch and yaw NovAtel solution and the SR UKF solution for the body frame velocity non centripetal acceleration and angular rates Frequency content of Notice the strong harmonic content at 0 217 0 427 0 637 and 0 847 rad sample 2 2 2 2 Frequency response of the low pass filter 2 2222 2222 NovAtel solution and the low pass filtered SR UKF solution for roll and Pitch fa y pul a aaa RU A a A The layout of the TS 7800 single board computer Included on the TS 7800 are six separate RS 232 uarts two USB ports an ethernet port an LCD port a DIO port an A D three bootup jumper options and an SD card slot underneath See text for further description The pin numbers of COMI lens The pin numbers of COM2 and COM3 Notice that the white circle indicates pin l serra RE amp ERR re pee UR oe Beek xxiii 92 XXIV Chapter 1 Introduction 1 1 Purpose Synthetic aperture radar SAR is a valuable tool with applications ranging from military to environmental There are many types of SAR but each relies on special processing techniques to form useful imagery These processing techniques require accurate measurements of aircraft speed and attitude to correct aberrations within SAR images For over a decade the Microwave Earth R
50. emote Sensing MERS team at Brigham Young University BYU has been developing SAR instruments and processing algorithms that incorporate these motion measurements With the addi tion of BYU s nu SAR system in 2008 an accurate standalone motion measurement system was needed In 2008 BYU purchased a costly motion dynamics estimation system from No vAtel Inc The system utilizes three orthogonal accelerometers and laser gyroscopes coupled with a high precision GPS system It also includes a CPU that processes the sensor and GPS data into a high precision navigation solution consisting of attitude and position information In order to decrease the dependence on expensive motion measurement systems BYU s MERS team determined a standalone recording pro cessing system was necessary and also wanted to develop processing algorithms that mimic the capability of a high end navigation system The purpose of this thesis is to present a system to couple high precision ac celerometers and laser gyroscopes with a high precision GPS system into an accurate onboard motion measurement system using the square root Unscented Kalman filter SR UKF presented by Merwe 1 Specifically this thesis addresses a motion record ing onboard device MOTRON used to record and time sync GPS accelerometer and gyroscope measurements as well as SR UKF techniques to optimally estimate aircraft attitude position and velocity from these measurements This thesis dem
51. erence Between SR UKF and NovAtel Attitude 67 5 10 Difference Between Filtered SR UKF and NovAtel Attitude 68 A 1 Interface Parameters ceo oma web xm a a el 91 A 2 Mapping of COM Port Pins o e 94 xix List of Figures 2 1 2 2 2 3 2 4 3 1 3 2 3 3 3 4 3 5 3 6 3 7 3 8 3 9 SAR uses multiple antenna pulses to create a synthetic antenna By combining each pulse return from an area high resolution images can e created ue ooo yes DOR eS eek OS EER Vene cR E deck 6 Any non ideal motion in an aircraft can cause serious problems for synthetic aperture processing a eee 7 Two of the most important frames of orientation a The world frame b The body frame ope Ge ae a we oe oe SS 9 The three Euler angles is roll 0 is pitch and v is yaw Each follow the right hand rule about the body frame coordinate axis 10 Schematic showing how the MOTRON SAR system NovAtel IMU Antenna Flash Drive and any additional sensors are connected to each othef ete s s Bh Ree ee ed eee bee RE 23 Connections of the NovAtel A Power Socket B To MOTRON C To SAR System D amp E To IMU F To Antenna 24 The antenna connected to the NovAtel ProPak V3 25 The IMAR iIMU FSAS connected to the NovAtel ProPak V3 25 The complete MOTRON The box size is approximately 6 x 4 x 11 27 MOTRON connections A Power
52. hat the MOTRON connects to each of the outside sensors specifically the NovAtel ProPak V3 in conjunction with an Inertial Measurement Unit IMU GPS antenna the SAR system and any additional sensors A schematic is shown in Fig 3 1 The next subsections include information about the various sensors used with the MOT RON 3 2 1 NovAtel ProPak V3 with IMAR IMU NovAtel s ProPak V3 is a high performance GPS receiver with 72 available channels L1 and L2 GLONASS USB communication and SPAN support Figure 3 2 illustrates the different connectors to the ProPak The ProPak connects to the IMU MOTRON SAR System for data syncing and to the GPS Antenna This system outputs GPS and inertial measurements through the USB serial port to the MOTRON It also outputs a time stamp to the SAR system in order to time sync the data in post processing The ProPak connects to the antenna shown in Fig 3 3 The GPS 701 GG antenna is part of NovAtel s GPS 700 antenna series and offers access to GPS L1 and L2 frequencies as well as GLONASS For added accuracy the antenna phase center remains constant as the azimuth and elevation angle of GPS satellites change Since 22 Flash Drive Serial RS422 Serial RS232 Serial RS232 Figure 3 1 Schematic showing how the MOTRON SAR system NovAtel IMU Antenna Flash Drive and any additional sensors are connected to each other signal reception is unaffected by the rotation of th
53. have been captured using the MOTRON and how to make the data useful for analysis 3 1 Requirements for the MOTRON The MOTRON is designed to support Synthetic Aperture Radar SAR sys tems A typical SAR system amasses enormous amounts of radar data that are later processed to produce images It is useful to have a separate motion measuring system to remove any unnecessary computation and memory usage that may further burden a SAR system It is also useful to collect motion data independent of radar Because there are no engineers onboard the aircraft when it collects radar data the MOTRON must be very easy to use The MOTRON must be able to collect data with little 21 or no human interaction during the flight and have a very user friendly interface for data extraction and set up Because the MOTRON is meant to be mounted to an aircraft it is important that its weight and size be minimized This also makes it a viable data capturing option for future smaller aircraft Since the sensors are independent of the MOTRON the MOTRON must effectively link to those sensors without losing any data This means that the data transfer rate must be sufficient to handle all of the incoming data Finally the MOTRON must have an efficient way to store all of the sensor data It is important that this data is easy to extract once recorded and readily available 3 2 Motion Sensors Given the requirements of the MOTRON this section considers the way t
54. hd bith I I Ht c eu l gt 4L i i h i i i 4 50 100 150 200 250 300 350 400 450 500 0 05 i E i i Lu 0 05 j i i i f i 50 100 150 200 250 300 350 400 450 500 Time s V freue 0 5 O 0 5 150 20 350 40 oo 5 05 i i i i i B ian ju e s e ET T cc S 05 i i i i i i f 50 100 150 200 250 300 350 400 450 oo 0 01 5 oo pst eee dide inn id f imn pn epis 0 01 1 1 1 1 1 50 100 150 200 250 300 350 400 450 500 Time s Figure 5 8 Truth data and the SR UKF solutions for angular rates p q and r See text and Table 5 5 for analysis 74 Jo Honeyuille A Un Google Image Statefof 2009 iTele 2009 Europa CR cg wei Imagery Date 2006 lat 41 545844 lon 112 078056 elev 4227 ft Eye alt 38857 ft Figure 5 9 The flight path of the MERS aircraft The drop pin indicates the starting position just before takeoff 75 Altitude m 1600 1400 NovAtel Solution 1200 1600 1400 1200 SR UKF Solution Difference 1 1 1 1 1 1 200 400 600 800 1000 1200 Time s Easting rn 5000 T T T T T T 5000 ovAtel Solution N 10000 5000 5000 SR UKF Solution 10000 0 5 T T r r r r Difference o 1 1 1 1 1 1 200 400 600 800 1000 1200 Time s Altitude 1600 1400 1200 1600 1400 1200 9 a o Difference m SR UKF Sol
55. he time it touched down or in other words only during flight This set of data is used because as the plane taxis on the runway it violates the assumption of a coordinated turn and has no angle of attack Fig 5 9 shows the overhead view of the MERS aircraft flight From the figure we can see that the plane flies several circles over the Brigham City area and then returns to the Brigham City Airport runway Figure 5 10 shows the position measurements of the MERS aircraft from takeoff to touchdown The top three figures show northing measurements provided by the SR UKF the NovAtel system and their difference The second set gives the easting measurements and the third gives altitude The plane starts at an altitude of 1275 meters the altitude of the Brigham City airport reaches an altitude of 1659 meters takes a dip to 1460 meters then returns to 1650 meters before returning and landing after a 20 minute flight The position measurements of the SR UKF are within 50 cm of the NovAtel solution Table 5 6 provides statistical data for the difference between the NovAtel solution and the SR UKF solution Table 5 6 Difference Between SR UKF and NovAtel Position m Measurement Mean RMS Standard Deviation Max Northing 0 0172 0 1997 0 1989 0 4646 Easting 0 0129 0 1502 0 1497 0 4780 Altitude 0 0028 0 0466 0 0465 0 4584 Figure 5 11 shows the NovAtel and SR UKF solutions for roll pitch and yaw respectively From the figures it is cle
56. he nonlinear function haps makes use of Equations 4 17 and 4 18 and is illustrated in Fig 4 4 48 oo 3 oo C ek Q N e m o AL 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 function y vel points pn points 1 pe points 2 pd points 3 u points 4 V points 5 w points 6 phi points 10 theta points 11 psi points 12 cphi cos phi sphi sin phi ctheta cos theta stheta sin theta cpsi cos psi spsi sin psi pndot ctheta cpsi u cphi stheta pedot ctheta spsi u cphi stheta hdot y vel points pn pe pd pndot h_GPS points sphi stheta cpsi cphi xspsi cpsi sphi spsi w sphi stheta spsi cphi cpsi spsi sphi cpsi w stheta u sphi ctheta v cphi ctheta x w pedot hdot psil Figure 4 4 Nonlinear function haps MATLAB script 49 AV AV oO so C rk U N Re BORO e e A e U ww Q Q Q Q Q Q Q M M M M NoD MOM M y LB DB LB on oH non fps n n D G BGN nooo oO ada 05250 00 100 amp Qo NRO oooO m RR mn HH Oo CO function sigma_star f points pn points 1 pe points 2 h points 3 u points 4 V points 5 w points 6 ncax points 7 ncay points 8 ncaz points eis y phi points 10 theta points 11 psi points 12 p points 13 q poin
57. here are four serial ports mounted to the back panel that can be used with additional sensors The MicroStrain 3DM GX1 is a smaller lighter and less accurate IMU that has been interfaced with the MOTRON The 3DM GX1 can be recorded simultaneously with the NovAtel ProPak to provide additional measurement sensors It connects via a standard RS232 protocol and contains three rate gyros three accelerometers and three magnetometers to determine orientation While the 3DM GX1 is less accurate than the NovAtel it may be advantageous for various applications 24 4 J 3 JUUUUUU tion de nar naviga Figure 3 4 The IMAR iIMU FSAS connected to the NovAtel ProPak V3 The MicroStrain IMU is only one example of many different types of sensors that can be easily recorded by the MOTRON Because there are four separate serial ports four additional sensor interfaces can be set up simultaneously Each additional 25 sensor can then be recorded to the same flash drive that all the other sensors are recorded to 3 3 The MOTRON In order to address each of the requirements previously stated careful choice of components design and algorithms were made The following subsections describe the components design user interface and algorithms of the MOTRON 3 3 1 Components and Design The MOTRON is based on a small ARM single board computer the TS 7800 made by Technologic Systems This computer utilizes a 500 Mhz ARM
58. ields Gx Q cos cos 0 cos Y Q sin C sin 0 4 28 giving T 7 Gx Qsin sin u QcosQcos 4 29 Therefore by solving for 0 then then v using Eqs 4 25 4 26 and 4 29 we have a method for obtaining the initial values of pitch roll and heading Initial velocities 44 are then set to zero This approach gives a consistent method for obtaining initial conditions 4 3 2 Time Update After computing initial conditions the first step is the sigma point calculation and time update shown by the equations below Xi 1 Lia i HNS 4 n9 i 4 30 Xia f Xi a 4 31 Diii 4 32 Siji qr VW Xil 2L Bir VQ gt 4 33 Sij cholupdate Sii X je 1 0 sign W gt 4 34 As shown in Eq 4 30 the SRUKF relies on a deterministic sampling approach to calculate mean and covariance terms This equation determines how those points are chosen Xi is an Lx2L 1 or 18 x 37 matrix where each column represents a sigma point In Eq 4 31 each of these columns or points are passed through the nonlinear function f and stored in x Typically the mean value is computed using a weighted average of the sigma points however in this implementation the new mean value is computed by simply passing the previous sample mean amp _ through f shown in Eq 4 32 The new Cholesky factor of the state covariance is calculated in two steps shown in Eqs 4 33 and 4 34 Firs
59. igure 5 12 shows the remaining states found in the SR UKF The body frame velocity measurements give a good metric as to the validity of our angle of attack and sideslip assumptions From the body frame velocity u v and w we can see that most of the overall velocity is in the u direction as we would expect It is also clear that the assumption of zero sideslip was not completely accurate from the v measurement as it clearly drifts away from zero Finally w has an almost constant velocity over the flight which gives credence to the constant angle of attack assumption The non centripetal acceleration is nearly zero as we would expect and the p q and r 65 measurements seem reasonable There is no NovAtel solution for these states so no error metrics are given Overall the SR UKF performs much better on the actual flight data than it did on the simulated flight data This is due to the fact the assumptions discussed earlier are much more valid for larger aircraft than the small UAV simulated case The results are very encouraging 5 3 2 A Smaller Portion of the Flight After viewing the measurements taken from the entire flight 1t is valuable to observe a set of data taken from a flight that more accurately reflects the assumptions that the SR UKF makes no sideslip constant angle of attack no wind etc This subsection discusses a smaller 10 minute portion of the MERS aircraft flight that closely meets these conditions Figure
60. ipetal acceleration in the i direction ajne Non centripetal acceleration in the 7 direction akne Non centripetal acceleration in the k direction Roll angle 0 Pitch angle w Heading angle p Angular rate along 1 q Angular rate along 7 r Angular rate along k B gyro bias B J gyro bias B k gyro bias 4 1 As shown in Fig 4 1 the i 7 k directions are in the body frame of the aircraft This means that is in the direction of the nose of the plane j is pointing out the right wing and k points out the bottom of the aircraft forming a right hand coordinate system It is also important to note that the angular rates p q r as well as the Euler angles 6 0 Y follow the right hand rule The position of the aircraft pn Pe and pa are given in the world frame y and 2 which correspond to northing 36 woo A E 9 ur ie n d i k 5 Figure 4 1 Body frame versus world frame i 7 and k represent the three orthogonal directions of the aircraft s body frame i y and 2 represent the three orthogonal directions in the world frame easting and negative altitude provided by GPS measurements respectively The body frame velocity u v w angular rates p q r non centripetal acceleration dine ajnc Aknc and gyro biases B Bj By are represented in the body frame 4 5 k 4 1 2 Reference Frames The body frame of the aircraft is related to the worl
61. is 0 10 20 30 40 50 60 0 10 20 30 40 50 60 Z Axis 0 10 20 30 40 50 Time in Seconds Figure 3 9 Stationary accelerometer readings 60 over 70 seconds x 10 Gyroscope Output 4 ls T T T RI 2 26 j lt 0 x oF 4r i i i i f i 4 0 10 20 30 40 50 60 x 10 4L N A a 2f J gt 2F 4 4 i i i i i i 4 0 10 20 30 40 50 60 x 10 AL J 2r J 1 SR N 2b 4r f A f 4 0 10 20 30 40 50 Time in Seconds Figure 3 10 Stationary gyroscope readings over 70 seconds 32 Table 3 2 Statistics of Measurements shown in Figures 3 9 and 3 10 Sensor Mean Value Standard Deviation Accel X 0 2682 0 0107 Accel Y 0 0078 0 0396 Accel Z 9 8000 0 0091 Gyro X 8 5849 x 107 7 0660 x 1074 Gyro Y 48611 x 1073 5 5001 x 1071 Gyro Z 4 6365 x 107 3 5382 x 1074 Latitude 40 2470 3 24736 x 107 Longitude 111 6480 2 1034 x 107 Altitude 1 4196 x 10 0 4948 N Velocity 0 0017 0 0030 E Velocity 0 0016 0 0051 U Velocity 0 0023 9 0529 x 1074 33 34 Chapter 4 Implementation the Square root Unscented Kalman Filter State estimation using the Kalman Filter is very useful for a wide range of ap plications In the application for which the MOTRON is designed imaging synthetic aperture radar SAR data the attitude and velocity measurements are crucial to forming high precision radar images Therefore it is extremely important th
62. is needed so by also computing its Jacobian we can use the Jacobian of h in place of matrix H This is generally referred to as the Extended Kalman Filter EKF Another approach to the nonlinear problem is the Unscented Kalman Filter UKF presented by Julier and Uhlmann 6 The UKF uses a deterministic approach to calculate mean and covariance terms Basically 2L 1 sigma points are chosen based on a square root decomposition of the prior covariance Each of these sigma points are then passed to the nonlinear function f without approximation A weighted mean and covariance is then taken from this new set of points Julier and Uhlmann 15 6 have shown that this approach results in approximations accurate to the third order Taylor series expansion for Gaussian inputs for all nonlinearities For non Gaussian inputs approximations are accurate to at least the second order The EKF s linearization approach on the other hand results only in first order accuracy The UKF chooses sigma points using the following formulas x amp y Pi t n9VP 2 20 and n VLo 2 21 where x is a set of 2L 1 sigma points and a is a constant that determines the spread of the sigma points around and is usually set to 0 0001 lt a lt 1 It also uses the following weights Wo Pu ag 2 c Q Wo 1 a 8 1 WU ewe ec 2 22 i i La2 where 8 2 if the system is Guassian Finally with these weights i
63. itch climbs higher than the SR UKF is allowed to The NovAtel solution is probably more accurate because it does not rely on the angle of attack assumption that the SR UKF does However the SR UKF is still only a degree or two away from the NovAtel solution This section of data from the SR UKF is very encouraging and the data shows that under the right conditions the SR UKF performs very well Table 5 9 Difference Between SR UKF and NovAtel Attitude Measurement Mean RMS Standard Deviation Max Roll 0 2268 2 2230 2 2115 10 0244 6 Pitch 0 2586 0 8876 0 8492 2 5067 w Yaw 0 3484 0 5591 0 4372 1 5505 Figure 5 15 shows the remaining states of the 10 minute section These mea surements have nothing to compare with but are given to show that they are rea sonable The SR UKF performed much better on this set of actual flight data This is due to the fact that the aircraft more closely met the approximations discussed previously 5 4 Filtered Results It is clear that even after Kalman filtering the optimal data the output of the SR UKF is still too noisy and as Fig 5 16 shows there is significant harmonic distortion in the roll angle estimate These harmonics are also found in the raw gyroscope and accelerometer data and are probably due to the intrinsic harmonic response of the aircraft 67 In order to correct these distortions a low pass filter is used on d and 0 A high order filter is found to be the most eff
64. l the SR UKF tracks the truth data relatively accurately Despite the fact that the simulated measurements frequently have large spikes the SR UKF is able to track the states through their nonlinear measurement functions Many of the approximations discussed previously such as the angle of attack sideslip and non centripetal acceleration assumptions are not as valid in the case of a small aircraft Sideslip significantly affects the accuracy of the results and because smaller aircraft without a rudder suffer from significantly more sideslip than larger aircraft do we see greater errors in the output of the Kalman filter than would occur otherwise Despite the somewhat extreme nature of the simulated flight the SR UKF is able to give reasonably low error estimates of attitude and position 5 3 Kalman Filtering Actual Flight Data With the SR UKF shown to work on the simulated data it is applied to real data from the MERS aircraft After motion data from a SAR recording flight is collected with the MOTRON the sensor recordings are prepared using the scaling 63 factors and GPS to northing easting conversion The SR UKF is then applied to the data The following sections discuss the overall flight measurements and a smaller section of the flight that seemed to most accurately meet the assumptions made by the SR UKF 5 3 1 Measurements of the Entire Flight The following data set is taken from the time the aircraft left the ground to t
65. l UAV The UAV starts at the center and flies a clockwise path around four different waypoints several times Truth data SR UKF solutions and SR UKF error for northing east ing and altitude See text and Table 5 1 for analysis Truth data and the SR UKF solutions for u v and w See text and Table 5 2 for analysis 4 24 44 4 rar Truth data and the SR UKF solutions for non centripetal acceleration in the x y and z directions See text and Table 5 3 for analysis Truth data and the SR UKF solutions for roll pitch and yaw See text and Table 5 4 for analysis a i223 4 456 8 kee ede ee Truth data and the SR UKF solutions for angular rates p q and r See text and Table 5 5 for analysis 2 2 22 non nn The flight path of the MERS aircraft The drop pin indicates the start ing position just before takeoff o NovAtel solution and the SR UKF solution for northing easting and altitude See text and Table 5 6 for analysis xxii 3T 46 48 49 50 52 56 57 59 70 A 72 73 74 75 76 5 11 5 12 5 13 5 14 5 15 5 16 5 17 5 18 A 1 A 2 A 3 NovAtel solution and the SR UKF solution for roll pitch and yaw See text and Table 5 7 for analysis 2 o NovAtel solution and the SR UKF solution for the body frame velocity non centripetal acceleration and angular rates See text for analysis NovAt
66. n place we can calculate the state mean and covariance time update using a deterministic approach 2L fas gt W AAA i 0 2L Pirie gt w Den ay Deut a Q 2 23 i 0 16 where X 441 represents the sigma points which have been passed through the non linear function f The measurement update is performed similarly 2L Py y wf Wiese Dell Vie Seal R i 0 2L Py Ww Day Pira ies 1e Gul i 0 u i Kiri PuP os where K denotes the Kalman gain for the UKF Y 14 is a new set of sigma points specifically the points from X 4414 which have been passed through the nonlinear function h This approach is different from the EKF in that it does not use a Jaco bian but instead uses a deterministic sampling to manually approximate the state covariance The computation involved is the same order of magnitude according to Merwe 1 This is due to the fact that the Jacobians are removed but they are re placed with deterministic outer products The final step of the measurement update is performed analogously to the linear Kalman filter Xue ual F Kiti B measured Yun Prien Po Koi Py Ke 2 25 2 4 4 Square Root Implementation of the UKF Because Kalman filter equations tend to be somewhat poorly conditioned nu merically many algorithms have been developed to address this problem which have better numerical conditioning One of these algorithms known as the square root
67. nd velocity errors causing serious problems for the SAR it becomes absolutely necessary to compensate for these errors It is unrealistic to think that an aircraft can fly such a straight course This is why great care is taken to mea sure non ideal aircraft movement so that the unexpected motion can be compensated for in processing algorithms 2 3 Aircraft Flight Dynamics In order to most effectively find inconsistencies in an aircraft s flight path it is important to understand the ways in which an aircraft s position can be represented This section briefly presents some basics behind coordinate frames kinematics dy namics and nonlinear equations of motion The notation used here is that used by Beard in 4 however this notation is common to most aeronautics literature 2 3 1 Coordinate Frames There are various coordinate systems that can be used to describe the posi tion velocity and orientation of aircraft Each of these coordinate systems can be a b Figure 2 3 Two of the most important frames of orientation a The world frame b The body frame transformed into the frame of another There are several reasons that multiple coor dinate frames need to be used First Newton s equations of motion are given in the coordinate frame attached to the aircraft Second aerodynamic forces are applied in the body frame of the aircraft Third onboard sensors measure information with respect to the body frame whil
68. ng quaternions the motion estima tion system is able to rotate freely without encountering this issue It would also eliminate the issues that can occur with 0 360 crossings e Implementation of a Kalman smoother The SR UKF estimates the state covari ance at each time and measurement update If each of those covariance updates were stored in memory a Kalman smoother could be easily implemented This would help to reduce the noise found in the results of the SR UKF and would be a much better alternative to a simple low pass filter e Verification of the SR UKF using SAR imagery If the data collected by BYU s nu SAR were motion compensated correctly using the output of the SR UKF it would be a strong indication that the motion estimates were correct e Move the SR UKF to operate in real time on the MOTRON By optimizing the SR UKF to run in C or C it could be greatly sped up and run in real time onboard the MOTRON for real time dynamics estimates 87 88 Bibliography 1 R van der Merwe and E A Wan The Square Root Unscented Kalman Filter for State and Parameter Estimation Acoustics Speech and Signal Process ing ICASSP IEEE Proceedings vol 6 pp 3461 3464 May 2001 xi 1 17 18 SPAN for OEMV User Manual 4th ed NovAtel Inc 1120 68 Avenue NE Calgary Alberta Canada T2E 8S5 2007 2 F T Ulaby R K Moore and A K Fung Microwave Remote Sensing Nor wood MA Artech Hou
69. o scale the data are given in Table 3 1 Multiplying the output of the IMU by these scale factors yields data that is much more useful By doing so we get the instantaneous angular rate in radians and instantaneous acceleration in meters per second at each sample It is also important to change the GPS readings found in INS txt into a more useful form First by changing ground track heading into radians by multiplying by 7 180 then by converting latitude and longitude into northing and easting This is accomplished by the MATLAB script shown in Fig 3 8 3 4 NovAtel Data Collected by the MOTRON After all of the data has been recorded from the NovAtel it must be formatted correctly so that we can begin to work with it An example data set is shown in Figs 3 9 and 3 10 Figure 3 9 shows the output of the accelerometers Figure 3 10 shows the output of the gyros during the same measurement period 3 4 1 Measurement Statistics As can be seen in Figs 3 9 and 3 10 the accelerometers and rate gyros are very sensitive and the signal to noise ratio is high The statistical attributes of these six measurements plus the GPS measurements are given in Table 3 2 Because the 30 mo_window kaiser length INS 2 5 oo oo OC A Q Ne mo_window mo_window mean mo window latO mean INS 2 mo_window lonO mean INS 3 Le nection ie altO mean INS 4 mo_window time0 me an INS 1 xmo window
70. on actual flight data The SR UKF created esti mates within a few centimeters of the position and within a few degrees of the attitude measurements of the NovAtel solution The SR UKF performs similarly on data sets from additional flights made by the MERS aircraft Most major discrepancies can 68 be accounted for in the approximations made to simplify the implementation of the SR UKF I found that the output of the SR UKF is very sensitive to the covari ance tuning and even after the SR UKF has run additional low pass filtering was necessary Most of the time I spent developing this SR UKF was spent tuning the covariance matrix incrementally making small changes to Q and testing the result It is also important to note that if either the sensors or aircraft are changed the parameters will need to be adjusted to account for the changes 69 Northing m 500 7 NAE A A OA ge 500 4 1000 1 f 1 1 1 1 1 50 100 150 200 250 300 350 400 450 500 S r r T r r r T r r r 3 500 4 B gt 500 7 cc B5 1000 1 1 1 1 1 1 1 1 1 50 100 150 200 250 300 350 400 450 500 2 T T T T T T T T T T EU us mr Lu 2 1 1 1 1 1 1 1 1 1 1 50 100 150 200 250 300 350 400 450 500 Time s Easting rn T T T T T T T T T T 500 4 Truth o 500 1 1 1 f 50 100 150 200 250 300 350 400 450 500 500 SR UKF Solution o 500 1 1 1 1 1 1 50 100 150 200 250 300 35
71. onstrates that the SR UKF can be a very effective tool for aircraft dynamics estimation Although the NovAtel processing system is much more sophisticated the SR UKF presented in this thesis approaches the effectiveness of the NovAtel system While it is expected that the techniques described by this thesis will not outstrip the performance of a 40 000 motion measurement unit it is shown that with creative methodology a much less complicated system implementing an effective type of Kalman filter can perform quite well at a lower cost 1 2 Contributions This thesis contributes to the base of estimation theory by presenting a stan dalone sensor recording scheme that can be used in any fixed wing aircraft platform This thesis further contributes by applying the SR UKF to flight dynamics and mo tion estimation Previously the extended Kalman filter EKF has been used with these sensors by NovAtel 2 to estimate motion measurements Comparison of the NovaTel filter and the SR UKF is also presented This analysis gives valuable insight into the functionality of the SR UKF for flight dynamics estimation 1 3 Outline The remainder of this thesis is organized into the following chapters e Chapter 2 gives a background on the need for motion measurement systems within SAR processing algorithms a brief survey of aircraft flight dynamics and a presentation of the SR UKF e Chapter 3 presents a motion sensor recording scheme in order to coll
72. put of the dynamics are fed into the simulated sensors block which generates a file yout mat This file contains the data from the virtual accelerometers gyroscopes and GPS Special care is taken to make the virtual sensors imitate the real sensors onboard the MERS aircraft by matching the variance of the simulated noise to that of the measured noise given in Chapter 3 The file xout mat contains all of the truth data from the dynamics block related to the Kalman filter states The flight dynamics block determines how the simulated aircraft responds to external stimulus It relies on a complex set of coefficients that model an aircraft s drag lift propeller thrust weight aerodynamics and inertia 8 9 Every aircraft has a distinct and unique set of coefficients and responds to autopilot commands differently For this reason the autopilot must be specifically tuned for each individual aircraft All of the noise parameters flight dynamics coefficients and simulation pa rameters are given in an initialization file param m which can be readily modified Additionally within the sensors block are the sensor models The sensor block takes the true position velocity forces angular rates and attitude of the aircraft and cre 56 UAV Model To File Goto u throttle elevator aileron rudder x pn pe pd u v w phi theta psi p q 1 Terminator To File Figure 5 2 The Simulink model of th
73. r time syncing If all systems are functioning correctly the screen shows the information in Fig 3 7 28 Figure 3 7 f shows the MOTRON s data display screen during data collection The screen is updated in real time as the plane flies This is performed with a multi threaded algorithm that takes in data through the USB and displays it at a much lower rate than it is received Simultaneously all of the data streams are recorded to text files on the inserted flash drive Two separate files are recorded One file INS txt records the GPS data at 50 Hz The second file RAW txt contains the IMU data at 200 Hz INS txt includes the time stamp latitude longitude altitude north east and up ground velocity ground heading and the NovAtel solutions to roll and pitch RAW txt includes the time stamp and output of the accelerometers and gyros The top of the IMU defines the coordinate system used by NovAtel y should point out the nose of the plane In order for the MOTRON to interface correctly with the NovAtel ProPak a new USB driver had to be cross compiled The new driver is a modification of the usbserial driver included in Linux 2 6 distributions Using this driver it is easy to implement a USB serial interface with the commonly used SerialStream C library Being able to modify the MOTRON is important To modify the MOTRON access to the command line is necessary There are two methods of gaining ac
74. rder to interface with the device a COM1 RS 232 connection is typically required A null modem cable is necessary to use the COM1 Terminal plugin An Ethernet jack is also available Table A 1 gives the information necessary to use COM1 and the Ethernet port Table A 1 Interface Parameters COM1 Serial Port Ethernet Port Baud 115 200 IP Address 192 168 0 50 Data Bits 8 Username root Parity none Password hello Flow Control none Stop Bits 1 Jumper 2 JP2 On Username root Password hello 2 On the topside of the board shown in Fig A 1 there are three jumpers If jumper 1 is installed meaning each of the jumper 1 pins are electrically connected than the board attempts to boot from an SD Card independent of the on board flash memory otherwise the board boots from the flash memory Two backup SD Cards have been created to safeguard the MOTRON s operation If jumper 2 is installed BB 5 a y e dune RB e ZNOOe JE ENOO a97 p i ah Figure A 1 The layout of the TS 7800 single board computer Included on the TS 7800 are six separate RS 232 uarts two USB ports an ethernet port an LCD port a DIO port an A D three bootup jumper options and an SD card slot underneath See text for further description 92 console output to COMI is turned on if it is not installed console output to COMI is turned off If jumper 3 is installed it causes th
75. rmulation of the Kalman Filter 4 444 264 x 14 XV 2 4 3 Non Linear Extensions of the Kalman Filter 2 4 4 Square Root Implementation of the UKF 2 5 Conclusi n 6 2482 264 424 6 ana has X ost GE ES Motion Recording Onboard Device MOTRON 3 1 Requirements for the MOTRON 3 2 Motion Sensors 2 2 0 4 a a ned e a 3 2 1 NovAtel ProPak V3 with IMARIMU 3 2 2 MicroStrain 3DM GX1 and Additional Functionality 3 3 he MOTRON a sc a te dob 4 id dE a A RS 3 3 1 Components and Design 2 22 2 lt lt 9 e s 3 3 2 User Interface Data Storage and Input Algorithms 3 3 3 Interpreting the Recorded NovAtel Data 3 4 NovAtel Data Collected by the MOTRON 3 41 Measurement Statistics nn anne 35 Conclusion nn Implementation the Square root Unscented Kalman Filter 4 1 Aircraft Flight Dynamics and State Variables 4 1 1 State Variables e 2 ud Sa eb a RRE 4 1 2 Reference Frames 2 2 229 24 m o naar 4 1 3 Equations of Motion 442 2 20 4 2 8 eu a ou ROUES 4 2 Measurement Sensors ee 4 3 Implementing a Square root Unscented Kalman Filter 4 3 1 Finding Initial Conditions rasen 4 3 2 Time Update woo 22 24 2 ee eee ee eb a eee eg 4 3 3 Measurement Update mann xvi 4 3 4 Nonlinear State Functions 0 0084 4 3 5 Simulated Measurements
76. s There are three COM ports on the TS 7800 which have six independent RS 232 uarts In the dev directory they appear as ttts0 tttsl ttyS0 ttySl ttts4 and ttts5 Each of these are labeled on the casing of the MOTRON On COMI the pins are labeled as shown in Fig A 2 and on COM2 and COM3 the pins are labeled as in Fig A 3 The mapping of these pins are given in Table A 2 It is important to note that ttySO maps to the terminal output and will not function properly as anything else unless jumper 2 is off Also ttts4 is the display input port and is wired directly to the LCD and keypad on the MOTRON 93 Table A 2 Mapping of COM Port Pins COM Port dev Transmit Pin Receive Pin COM1 ttts 7 8 ttts1 4 1 ttyS0 3 2 COM2 ttyS1 3 2 COM3 ttts4 3 2 ttts5 7 8 A special USB Serial driver was complied to allow functionality with the No vAtel s USB features This kernel module can be located in the kernel directory under the name usbserial ko This file is different than the usual file that typically comes with Debian installations This file has been backed up to each SD Card and does not ever need to be recross compiled Each time a USB device is attached the TS 7800 automatically detects it and assigns it to a ttyUSB device name which can be found in the dev directory From that point the USB serial device is treated just like any other serial device A 3 MOTRON Operating Source Code
77. se 1981 7 R W Beard and T W McLain Guidance and Control of Autonomous Fixed Wing Air Vehicles May 2009 a reference for flight dynamics state estimation and path planning 8 11 39 T K Moon and W C Stirling Mathematical Methods and Algorithms for Signal Processing Upper Saddle River NJ Prentice Hall 2000 13 S J Julier and J K Uhlmann A New Extension of the Kalman Filter to Non linear Systems Int Symp Aerospace Defense Sensing Simul and Controls vol 92 no 3 pp 401 422 1997 15 16 R W Beard personal communication 2009 55 J Roskam Airplane Flight Dynamics and Automatic Flight Controls Lawrence KS Design Analysis and Research Corporation 2001 56 B L Stevens and F L Lewis Aircraft Control and Simulation Hoboken NJ John Wiley and Sons Inc 2003 56 TS 7800 Manual 1st ed Technologic Systems Inc Fountain Hills AZ 85268 2009 91 89 90 Appendix A Hardware Implementation The MOTRON s basic components consist of a TS 7800 single board com puter a small LCD screen and a numeric entry keypad This appendix discusses the particular way in which these devices are interconnected setup and operated A 1 Basic Operation of the TS 7800 The MOTRON relies primarily upon a single board computer the TS 7800 made by Technologic Systems Inc 10 The TS 7800 has no video controller or keyboard interface This is to keep the board size small and cost low In o
78. stly it is easy to see that the SR UKF estimates track all of the trends of the truth data Roll is the most difficult to track accurately because it depends greatly upon the body frame velocity measurements and sideslip If there is significant 61 Table 5 3 Non centripetal Acceleration Errors m s Direction Mean Error RMS Error Standard Deviation Max Error x 0 0659 0 3209 0 3141 2 8365 y 0 0029 0 3112 0 3112 5 4124 Z 0 0113 2 2606 2 2607 27 5543 sideslip which is assumed to be zero in the SR UKF model it introduces problems for the SR UKF From Fig 5 7 amp has the greatest error when the UAV takes a large turn During a coordinated turn when an aircraft banks to turn it must roll to the right or left It is also during a coordinated turn that sideslip is introduced in the case of a small UAV without a rudder the sideslip can be severe Sideslip coupled with extreme acceleration spikes can be disastrous to the flight model however the SR UKF still performs very well in tracking these measurements Statistical properties of the attitude errors are given in Table 5 4 It is also important to note that the mean error in 0 is due to the constant angle of attack assumption made in the synthetic measurements within the SR UKF Pitch is closely related to w so due to the non zero mean error in w a non zero mean error is expected in 0 While the angle of attack assumption causes this discrepancy it is still valuable to
79. t the Cholesky factor S is calculated using a QR decomposition of the compound matrix square root of the additive process noise covariance The subsequent Cholesky update or downdate in Eq 4 34 is necessary since the zero th weight Wj can be negative with different values of a Fig 4 2 illustrates MATLAB script to accomplish the time update portion of the SR UKF 45 1 function new xhat new_Si timeupdate old_xhat old_Si 2 3 L number_of_states 4 T sample_time 5 old sigma points old xhat old xhat ones 1 L tetaxold_Si 6 old xhat ones 1 L etaxold Si 7 new sigma points f old sigma points 8 new xhat new sigma points 1 9 Junk new Si qr Wci x new sigma points 2 end 10 new xhat ones 1 Lx2 chol 0xT 11 new_Si new Si 1 L 12 if Wcl gt 0 13 Sip cholupdate new_Si Wcl x new sigma points 1 14 new xhat 15 p 0 16 else 17 Sip p cholupdate new_Si Wclx new_sigma_points 1 18 new xhat 19 end 20 if p 0 21 fprintf new Si is negative definite 22 else 23 new Si Sip 24 end Figure 4 2 Time update MATLAB script 4 3 3 Measurement Update The measurement update equations follow a similar pattern 4 1 h zii 1 Yii h Xj a Sy qr V WE Qia1 2L ji 1 VR gt S cholupdate Sy Ysi 1 0 sign Wws 2L T Pay 2 Wi Xipi 1 k amp Xaii k 4i
80. the SR UKF As expected the greatest maximum error is in v Table 5 2 Body Frame Velocity Errors m s Measurement Mean Error RMS Error Standard Deviation Max Error u 0 0562 0 1243 0 1109 0 7704 v 0 0067 0 2980 0 2980 5 1275 w 0 2591 0 4998 0 4274 2 7926 Figure 5 6 shows the non centripetal acceleration measurements and truth data in the x y and z directions from top to bottom respectively A large amount of error is expected in the acceleration measurements Here we assume that the non centripetal acceleration is zero which of course is not true Because of the large acceleration component in the simulated z axis the measured acceleration assumed to be close to zero and the actual acceleration have large differences However this assumption helps to keep the velocity and position in check If the acceleration is allowed to drift from zero too much it has extreme effects on position and velocity So as shown in all three acceleration measurements the non centripetal acceleration is always close to zero The actual acceleration has very large spikes which tend to cause large aberrations in other measurements such as attitude and body frame velocity as shown earlier These spikes are due to the extreme nature of the simulator output for the UAV s flight pattern Table 5 3 illustrates the acceleration errors in each direction Figure 5 7 illustrates the measurements of the roll pitch and yaw angles and their errors Fir
81. the SR UKF as a whole Finally the heading is kept in check by the GPS heading measurement The assumption that GPS heading and yaw angle are the same is only true in a zero wind environment The simulator does not introduce any wind into the UAV s flight environment so this assumption is better in simulation than it is in an actual flight Table 5 4 Attitude Errors Angle Mean Error RMS Error Standard Deviation Max Error Roll 0 0686 3 4242 3 4237 21 5885 0 Pitch 1 3137 2 5649 2 2031 19 1165 Y Yaw 0 0170 0 8090 0 8088 6 4115 62 Figure 5 8 shows the measurements and truth data related to the angular rates p q and r It is clear from the graphs that p q and r are tracked quite accurately This is due to the fact that they are directly measured by the onboard gyroscopes On the MERS aircraft the gyroscopes are very accurate and so the simulated gyroscopes are set to be very accurate as well The most significant error in angular rate is found in the measurement of q This is because of the extreme acceleration and pitching of the aircraft discussed previously The error however is still relatively small Statistical analysis of the errors are given in Table 5 5 Table 5 5 Angular Rate Errors rad s Measurement Mean Error RMS Error Standard Deviation Max Error p 5 613x10 3 0711x10 3 0196x10 1 0 0084 q 0 0018 0 0044 0 0040 0 0462 r 3 1391x10 8 8519x1074 8 2770x10 4 0 0065 Overal
82. theta sin theta 21 ctheta cos theta 22 sphi sin phi 23 cphi cos phi 24 25 y acceli ncax q w r v g stheta 26 y accelj ncay r u p w g ctheta x sphi 27 y accelk ncaz p v q xu gx ctheta xcphi 28 29 y gyroi p Bi 30 y gyroj q Bj 31 y gyrok r Bk 32 33 vector sqrt u 2 v 72 w 2 34 psidot g vector tan phi 35 36 y IMU_points y acceli y accelj y accelk y gyroi 37 y gyroj y gyrok u v w psidot 0 0 0 Figure 4 6 Nonlinear function hrmy MATLAB script 4 4 Conclusion This chapter has presented a method for modeling aircraft flight dynamics and a method for applying the SRUKF for optimizing attitude and position data I have proposed a set of state variables that can be used to model aircraft dynamics derived a set of equations to define the motion of the aircraft and proposed a Kalman filter 52 to optimize gyroscope accelerometer and GPS readings in synchronization with our model 53 54 Chapter 5 Results Once an actual flight has been recorded by the MOTRON the SR UKF can be applied to the collected sensor data By applying the SR UKF to the sensor data a completely new data set is created that contains estimates of the aircraft s position speed and attitude which can be compared to the NovAtel estimates This chapter presents the results of the SR UKF implementation discussed in Ch
83. ts 14 TE points 15 Bi points 16 Bj points 17 Bk points 18 cphi cos phi sphi sin phi ctheta cos theta stheta sin theta ttheta tan theta sectheta sec theta Vo KV A eK oe oa N cpsi cos psi spsi sin psi pndot ctheta cpsi x u sphi stheta cpsi cphi xspsi cphi stheta cpsi sphi spsi w pedot ctheta spsi u sphi stheta spsi cphi cpsi cphi stheta spsi sphi cpsi w hdot stheta u sphi ctheta v cphi ctheta x w udot ncax q w r xv vdot ncay r u p w wdot ncaz p v q u phidot p sphi ttheta q cphi ttheta x r thetadot cphi q sphi xr psidot sphi sectheta q cphi sectheta x r sigma_star pn Tx pndot pe tT pedot h Tx hdot ut Txudot v Tx vdot wtTxwdot ncax Tx0 1xncax ncay T 0 1 ncay ncaz Tx0 lxncaz phitT xphidot theta T thetadot psi Tx psidot p q r Bi Bj Bk Figure 4 5 Nonlinear function f MATLAB script 50 4 3 5 Simulated Measurements In order to use additional information about the aircraft s dynamics a few synthetic measurements are used to further correct the output of the Kalman filter These synthetic measurements take advantage of the following formulas Utotal V u v w g Y Utotal tan 4 45 where g is the lift acceleration against gravi
84. ty 9 8 m s Equation 4 45 means that an aircraft cannot turn unless it rolls to the right or left This is called a coordinated turn The second synthetic measurement is to correct the body frame velocity This synthetic measurement is probably the most inaccurate so special care should be taken when tuning the measurement noise covariance matrix The variance of these three should be tuned sufficiently high so as to not provide false accuracy to the overall output It is given by the equations U Utotal COS A v 0 W Viotal SIN O 4 46 where a is the angle of attack For larger aircraft o is relatively small 0 5 but for some small aircraft a is larger 5 10 Finally a third simulated measurement is meant to keep the non centripetal acceleration in check The simulated measurement simply says that the non centripetal acceleration is zero With this insight and Eqs 4 15 and 4 16 we can construct hr the second h output function Figure 4 6 shows the MATLAB script to accomplish this 51 1 function y IMU points h_IMU points 2 3 g 9 8 4 5 u points 4 6 V points 5 7 w points 6 8 ncax points 7 9 ncay points 8 10 ncaz points ur 5 11 phi points 10 12 theta points 11 13 p points 13 14 q points 14 15 r points 15 16 Bi points 16 17 Bj points 17 18 Bk points 18 19 20 s
85. uency and foe lt Den 2 2 foo where fp is the Doppler error component and e is a limit bound allowed in the Doppler frequency Considering an example aircraft with e 0 1 fpo 1 Hz and A 3 cm the limit is found to be Uy Sin 0 uz cos0 lt 3mms 2 3 In addition the errors in acceleration must also be compensated This essentially means compensating for changes in velocity While fixed velocity errors affect the presumed direction of the radar beam acceleration errors affect the focus of the beam because varying Doppler shift causes the desired signal to drift out of the Doppler filter band Using the same example aircraft it can be shown that a sin 8 a cos 0 lt 0 006m s 2 4 meaning that any acceleration error must be within 6 1 x 107 g which is extremely sensitive Furthermore an aircraft may roll pitch and yaw Each of these movements cause variation in the SAR return which can lead to errors in image reconstruction Rolling alters the gain for a particular point on the ground If the antenna rolls too much this can be a serious problem but it is not as severe as any yawing or pitching If an aircraft yaws or pitches while recording SAR data it moves the illuminated area away from a side looking direction to a direction that is slightly ahead of or behind the plane This causes distortions in the image and shifts the Doppler frequencies away from the broadside of the plane With tiny attitude a
86. urrently from a very large antenna This process creates a synthetic aper ture much larger than the length of the antenna or aircraft Image resolution of SAR is proportional to the radio signal bandwidth and depends upon the post processing techniques used Constructing a SAR image usually requires significant computa tional resources Often image construction is done on the ground well after the flight has been made 2 2 2 The Need for Motion Compensation Because synthetic aperture processing usually depends upon the assumption that the radar is traveling horizontally in a straight line at constant speed any move ment outside of that line is not ideal Errors in acceleration or attitude in an aircraft s flight can cause serious problems for the SAR Consider the example illustrated by Fig 2 2 Because the acceleration and attitude are non ideal the radar return is not what it is desired to be As a result the image is defocused and displaced According nad actual antenna footprint range Figure 2 2 Any non ideal motion in an aircraft can cause serious problems for syn thetic aperture processing assumed antenna footprint to Ulaby 3 the bounds on the size of the error velocities u height and u side to side are given by Uy Sin u cos0 lt Afpoer 2 1 where 0 is the angle between the vertical and radar beam such that cos 0 ae A is the wavelength of the beam fpo is the desired Doppler freq
87. ution m NovAtel Solution m 9 a 1 1 1 1 1 1 200 400 600 800 1000 1200 Time s Figure 5 10 NovAtel solution and the SR UKF solution for northing easting and altitude See text and Table 5 6 for analysis 16 Phi deg 5 40 F T T T T T T y 5 c 3 lt gt o 2 LB ce c LL S T cc c 3 D D O 20 1 1 1 1 1 1 200 400 600 800 1000 1200 Time s Theta deg Ss 10 T T T T T T 5 eo 5 c D a O o 5 s 10 5 B 5 S gt o S 5 5 a o D m o 5 1 1 1 1 1 1 200 400 600 800 1000 1200 Time s Psi deg T T T L 5 300 ce 2 200 D 100 o o 200 400 600 Ss r r r 300 B u 200 gt 100 51 o 1 200 400 600 2 T T T T T T a 1 2 5 o 1 1 1 1 1 1 1 200 400 600 800 1000 1200 Time s Figure 5 11 NovAtel solution and the SR UKF solution for roll pitch and yaw See text and Table 5 7 for analysis TT Body Frame Velocity m s 1 1 1 1 1 1 200 400 600 800 1000 1200 Time s Non centripetal Acceleration m s 2 T T T T T T gt N 2 1 1 1 1 1 1 200 400 600 800 1000 1200 Time s Angular Rates rad s 0 2 T T T T T T ce c o 1 1 1 1 1 1 200 400 600 800 1000 1200 Time s Figure 5 12 NovAtel solution and the SR UKF solution for the body fram

5 MB PDF - BYU MERS Laboratory

Contents

Download Pdf Manuals

Related Search

Related Contents