Tham khảo tài liệu 'innovations in intelligent machines 1 - javaan singh chahl et al (eds) part 11', kỹ thuật - công nghệ, cơ khí - chế tạo máy phục vụ nhu cầu học tập, nghiên cứu và làm việc hiệu quả | State Estimation for Micro Air Vehicles 193 We can compute the evolution for P as P E xxt dt E x xt xx t e axxt G xT xxT at xf gt AP PAT GE X t E xệt Gt where E xt t x0eA 4 í Ệ t ỆT t GteA t-T dr 0 2QGT which implies that P AP PAt GQGt . At Measurements. At a measurement we have that x x x x x- L Cx r ex- x- LCX- Lq. Therefore P e x x t E I x- LCX- Lrì x- LCX- LqỴr I E ĩ-x-T x-x-TCtLt x-rTLT LCx-x-T LCx-x-T Ct Lt LCx-rT LT Lqx-T Lqx-T c t Lt LrrT LT P - P -cT Lt LCP - LCP - C t Lt LRLt . 25 Our objective is to pick L to minimize tr P . A necessary condition is d tr P P-CT P-CT 2LCP - Ct 2LR 0 dL 2L R CP-CT 2P-cT L P-CT R CP - Ct -1. 194 . Beard Plugging back into Eq. 25 give P P - P-CT R CP-CT -1CP - - P-CT R CP-CT -1 CP - P-CT R CP-CT -1 CP-CT R R CP-CT -1CP- P- - P-CT R CP-CT -1CP- I - P-CT R CP-CT -1C P- I - LC P-. Extended Kalman Filter. If instead of the linear state model given in 24 the system is nonlinear . x f x u GỆ 26 yk h xk nk then the system matrices A and C required in the update of the error covariance P are computed as A x xx x C x sX x . The extended Kalman filter EKF for continuous-discrete systems is given by Algorithm 2. Algorithm 2 Continuous-Discrete Extended Kalman Filter 1 Initialize x 0. 2 Pick an output sample rate Tout which is much less than the sample rates of the sensors. 3 At each sample time Tout 4 for i 1 to N do Propagate the equations. 5 x x Tn f x u 6 A ĨịX X dx 7 P P Tu AP PAT GQGt 8 end for 9 if A measurement has been received from sensor i then Measurement Update 10 Ci dh X _ 11 Li PCT Ri CiPCT -1 12 P I - LiCi P 13 f f Li yi Cif . 14 end if State Estimation for Micro Air Vehicles 195 6 Application of the EKF to UAV State Estimation In this section we will use the continuous-discrete extended Kalman filter to improve estimates of roll and pitch Section and position and course Section . Roll and Pitch Estimation From Eq. 3 the equations of motion for ị and 9 are given by ị p q sin ị tan 9 r cos ị tan 9 .
