UAVBook Supplement Full State Direct and Indirect EKF

Transcription

1 UAVBook Supplement Full State Direct and Indirect EKF Randal W. Beard March 14, 217 This supplement will explore alternatives to the state estimation scheme presented in the book. In particular, we will derive a full state estimation scheme using both the direct Kalman filter and the indirect Kalman filter. We begin by focusing on the longitudinal states of the aircraft. 1 Direct Kalman Filter for Longitudinal States 1.1 Aircraft and sensor models The model for the dynamics of the aircraft are similar to that presented in the book after removing the lateral states. In particular, Equations become ṗ n = u cos θ + w sin θ ḣ = u sin θ w cos θ u = qw + f x m ẇ = qu + f z m θ = q. We will assume that the sensors that are available are the y-axis gyroscope, the x- and z-axis accelerometers, the static and differential pressure 1

2 sensors, the GPS-north measurement, and the GPS-ground-speed measurement. The models for the sensors, adapted from Equations 7.3, 7.5, 7.9, 7.1, 7.18, and 7.21 are given by y accel,x = f x m + g sin θ + η accel,x y accel,z = f z m g cos θ + η accel,z y gyro,y = q + b y + η gyro,y y abs pres = ρgh AGL + η abs pres y diff pres = ρv a η diff pres y GPS,n = p n + ν n [n] y GPS,Vg = V a + w n + η V. where b y is the unknown bias on the y-axis gyroscope. 1.2 Propagation model for the EKF The longitudinal states will be defined as x = p n, h, u, w, θ, b y, w n. Given the fact that the accelerometers measure total force minus gravity, we can write the equations of motion for x as ṗ n = u cos θ + w sin θ ḣ = u sin θ w cos θ u = y gyro,y b y w g sin θ + y accel,x wη gyro,y η accel,x ẇ = y gyro,y b y u + g cos θ + y accel,z + uη gyro,y η accel,z θ = y gyro,y b y + η gyro,y ḃ y = ẇ n =. 2

3 and If we define u cos θ + w sin θ u sin θ w cos θ fx, y = y gyro,y b y w g sin θ + y accel,x y gyro,y b y u + g cos θ + y accel,z y gyro,y b y then we can write where and G = w 1 u 1, 1 ẋ = fx, y + Gxη s + η d, y gyro,y y = y accel,x y accel,z η gyro,y η s = η accel,x η accel,z is the noise associated with the gyros and accelerometers, and where η i is a noise term associated with model uncertainty. The Jacobian of f can be shown to be cos θ sin θ [ u sin θ + w cos θ] sin θ cos θ [u cos θ + w sin θ] Ax, y = f x = y gyro,y b y g cos θ w y gyro,y b y g sin θ u. 1 3

4 The covariance of the noise term Gη s + η d is given by E[Gη s + η i Gη s + η i ] = GQ s G + Q d, where Q s = diag [σgyro,y, 2 σaccel 2 x, σgyro 2 z ], Q d = diag [σp 2 n, σh, 2 σu, 2 σv, 2 σθ, 2 σb 2 y, σw 2 n ], and where Q d are tuning parameters. 1.3 Sensor Update Equations Static Pressure Sensor The model for the static pressure sensor is Therefore, the Jacobian is given by h static x = ρgh. C static x = ρg Differential Pressure Sensor The model for the differential pressure sensor is h diff x = 1 2 ρv 2 a, where Va 2 = u 2 r + wr, 2 and where u r and w r are the wind relative velocities given at the top of page 57 in the uavbook. For the longitudinal case where w d = we can write the relative velocities as Therefore u r = u w n cos θ w r = w w n sin θ. h diff x = 1 2 ρu w n cos θ 2 + w w n sin θ 2, with an associated Jacobian of C diff x = ρ u r w r u r w n sin θ w r w n cos θ u r cos θ w r sin θ. 4

5 1.3.3 GPS North sensor The model for the GPS north sensor is with associated Jacobian GPS ground-speed sensor h GPS,n x = p n C GPS,n x = 1. The ground-speed is the projection of the inertial velocity vector u, w on to the ground plane. Therefore with associated Jacobian h GPS,Vg x = u cos θ + w sin θ C GPS,Vg x = cos θ sin θ u sin θ + w cos θ. 1.4 Algorithm for Direct Kalman Filter We assume that the gyro, accelerometers, and pressure sensors are sampled at rate T s, which is the same rate that state estimates are required by the controller. We also assume that GPS is sampled at T GPS >> T s. The algorithm for the direct Kalman filter is given as follows.. Initialize Filter. Set ˆx = p n, h, V a,,,,, P = diage 2 p n, e 2 h, e 2 u, e 2 w, e 2 θ, e 2 b y, e 2 w n, where e is the standard deviation of the expected error in. 1. At the fast sample rate T s 1.a. Propagate ˆx and P according to ˆx = fˆx, y P = Aˆx, yp + P A ˆx, y + GˆxQ s Q ˆx + Q d 5

6 1.b. Update ˆx and P with the static pressure sensor according to L = P C staticˆx /σ 2 abs pres + C static ˆx P C staticˆx P + = I LC static ˆx P ˆx + = ˆx + Ly abs pres h static ˆx. 1.c. Update ˆx and P with the differential pressure sensor according to L = P C diffˆx /σ 2 diff pres + C diff ˆx P C diffˆx P + = I LC diff ˆx P ˆx + = ˆx + Ly diff pres h diff ˆx. 2. When GPS measurements are received at T GPS : 2.a. Update ˆx and P with the GPS north sensor according to L = P C GPS,nˆx /σ 2 GPS,n + C GPS,n ˆx P C GPS,nˆx P + = I LC GPS,n ˆx P ˆx + = ˆx + Ly GPS,n h GPS,n ˆx. 2.b. Update ˆx and P with the GPS ground-speed sensor according to L = P C GPS,V g ˆx /σ 2 GPS,V g + C GPS,Vg ˆx P C GPS,V g ˆx P + = I LC GPS,Vg ˆx P ˆx + = ˆx + Ly GPS,Vg h GPS,Vg ˆx. Results for the direct Kalman filter are shown in Figure 1. Red is the commanded signal, blue is truth, and green is the estimate. It can be seen from the figure that, with the possible exception of the ground-speed V g, the filter performs well. The simulation files associated with this example are included on the uavbook website. 6

7 25 2 p n h V g V a α b y w n θ δ t q δ e Figure 1: Estimation results for the direct Kalman filter. 7

8 2 Indirect Kalman Filter for Longitudinal States For the indirect Kalman filter, the idea is to filter the error states, which should satisfy the linear and Gaussian assumptions better than the direct implementation. Let x be the true state, ˆx the estimated state, and let x = x ˆx be the error state. If x satisfies the differential equation ẋ = fx, y + Gxη s + η i where all definitions are identical to the previous section, and if ˆx satisfies ˆx = fˆx, y, then the error state evolves according to x = fx, y + Gxη s + η i fˆx, y. Using the Taylor series expansion up to the linear term fx, y fˆx, y + Aˆx, y x Gxη s Gˆxη s gives x = Aˆx, y x + Gˆxη s + η i. Note that the covariance of the noise term E[Gˆxη s + η i Gˆxη s + η i ] = GˆxQ s Gˆx + Q i, where Q i is a tuning parameter similar to Q d for the direct EKF. The basic idea is to run the Kalman filter on the error state, and then to correct the state estimate at each step by adding in the error. The error state is reset to zero after each correction. The algorithm is outlined below. 2.1 Algorithm for the Indirect Kalman Filter The algorithm for the indirect Kalman filter is given as follows. Terms that differ from the direct Kalman filter are highlighted in blue.. Initialize Filter..1 Initialize the state estimate according to ˆx = p n, h, V a,,,,. 8

9 .2 Initialize the error state estimate according to x=,,,,,,, P = diage 2 p n, e 2 h, e 2 u, e 2 w, e 2 θ, e 2 b y, e 2 w n, where e is the standard deviation of the expected error in. 1. At the fast sample rate T s 1.a. Propagate the estimated state ˆx according to 1.b. Propagate x and P according to ˆx = fˆx, y x= Aˆx, y x P = Aˆx, yp + P A ˆx, y + GˆxQ s Q ˆx + Q d 1.c. Update x and P with the static pressure sensor according to L = P C staticˆx /σ 2 abs pres + C static ˆx P C staticˆx P + = I LC static ˆx P x + = x + Ly abs pres h static ˆx C static ˆx x. 1.d. Update x and P with the differential pressure sensor according to L = P C diffˆx /σ 2 diff pres + C diff ˆx P C diffˆx P + = I LC diff ˆx P x + = x + Ly diff pres h diff ˆx C diff ˆx x. 2. When GPS measurements are received at T GPS : 2.a. Update x and P with the GPS north sensor according to L = P C GPS,nˆx /σ 2 GPS,n + C GPS,n ˆx P C GPS,nˆx P + = I LC GPS,n ˆx P x + = x + Ly GPS,n h GPS,n ˆx C GPS,n ˆx x. 2.b. Update x and P with the GPS ground-speed sensor according to L = P C GPS,V g ˆx /σ 2 GPS,V g + C GPS,Vg ˆx P C GPS,V g ˆx P + = I LC GPS,Vg ˆx P x + = x + Ly GPS,Vg h GPS,Vg ˆx C GPS,Vg ˆx x. 9

10 2.c. Update ˆx according to ˆx + = ˆx + x +. 2.d. Set x =,,,,,,. Results for the indirect Kalman filter are shown in Figure 2. Red is the commanded signal, blue is truth, and green is the estimate. It can be seen from the figure that, with the possible exception of the ground-speed V g, the filter performs almost identical to the direct KF. We should note that the results are obtained when the estimator is in the feedback loop. The simulation files associated with this example are included on the uavbook website. 2 p n h V g θ q V a α b y w n 5 δ t δ e Figure 2: Estimation results for the indirect Kalman filter. 1

11 3 Direct Kalman Filter for Full States In this section we will expand upon the previous discussion to include the full aircraft state. 3.1 Aircraft and sensor models The model for the dynamics of the aircraft are similar to that presented in the book. If we define p = p n, p e, p d, v = u, v, w, Θ = φ, θ, ψ, ω = p, q, r, then we can write the dynamics as ṗ = RΘv v = v ω + a + Rg Ω = SΘω ḃ = 3 1 ẇ = 2 1, where we have used the notation a c b b = c a, c b a and where c θ c ψ c θ s ψ s θ RΩ = s φ s θ c ψ c φ s ψ s φ s θ s ψ + c φ c ψ s φ c θ c φ s θ c ψ + s φ s ψ c φ s θ s ψ s φ c ψ c φ c θ 1 sin φ tan θ cos φ tan θ SΩ = cos φ sin φ, sin φ sec θ cos φ sec θ and where we have used the model for the accelerometer in Equation 7.1 to get 1 m f = a + R Θg, where g =,, g. 11

12 We will assume that the sensors that are available are the gyroscopes, the accelerometers, the static and differential pressure sensors, the GPS north and east measurements, and the GPS ground-speed and course measurement. The models for the sensors are given by y accel = a + η accel y gyro = ω + b + η gyro y abs pres = ρgh AGL + η abs pres y diff pres = ρv a η diff pres y GPS,n = p n + ν n [n] y GPS,e = p e + ν e [n] = V a cos ψ + w n 2 + V a sin ψ + w e 2 + η V y GPS,χ = atan2v a sin ψ + w e, V a cos ψ + w n + η χ. y GPS,Vg 3.2 Propagation model for the EKF Using the gyro and accelerometer models, the equations of motion can be written as Defining ṗ = RΘv v = v y gyro b η gyro + y accel η accel + R Θg Ω = SΘy gyro b η gyro ḃ = 3 1 ẇ = 2 1. x = p, v, Θ, b, w y = y accel, y gyro, we get ẋ = fx, y + G g xη gyro + G a η accel + η, 12

13 where RΘv v y gyro b + y accel + R Θg fx, y = SΘy gyro b , 3 3 v G g x = SΘ G a = I , and where η is the general process noise associated with the model uncertainty and assumed to have covariance Q. The Jacobian of f is [RΘv] 3 3 RΘ Θ Ax, y = f x = 3 3 y gyro b [R Θg] v Θ 3 2 [SΘy 3 3 gyro b] 3 3 SΘ Θ It is straightforward to show that when z R 3 and Az is a 3 3 matrix whose elements are functions of z, then for any w R 3 [Azw] z = Az z 1 w, Az z 2 w, Az z 3 w. 13

14 Therefore [RΘv] Θ [ R Θg ] Θ [SΘy gyro b] Θ = = = RΘ φ v, RΘ θ v, R Θ φ g, R Θ θ g, SΘ y φ gyro b, RΘ ψ v R Θ g ψ SΘ y θ gyro b, cos θ = g cos θ cos φ sin θ sin φ cosθ sin φ sin θ cos φ SΘ ψ gyro b, where RΘ φ is the matrix where each element of RΘ has been differentiated by φ. The covariance of the noise term G g η gyro + G a η accel + η is E[G g η gyro +G a η accel +ηg g η gyro +G a η accel +η ] = G g Q gyro G g +G a Q accel G a +Q, where and where Q gyro = diag[σ 2 gyro,x, σ 2 gyro y, σ 2 gyro z ] Q accel = diag[σ 2 accel,x, σ 2 accel y, σ 2 accel z ], Q = diag[σ 2 p n, σ 2 p e, σ 2 p d, σ 2 u, σ 2 v, σ 2 w, σ 2 φ, σ 2 θ, σ 2 ψ, σ 2 b x, σ 2 b y, σ 2 b z, σ 2 w n, σ 2 w e ] are tuning parameters. 3.3 Sensor Update Equations Static Pressure Sensor The model for the static pressure sensor is Therefore, the Jacobian is given by h static x = ρgh = ρgp d. C static x =,, ρg,,,,,,,,,,,. 14

15 3.3.2 Differential Pressure Sensor The model for the differential pressure sensor is h diff x = 1 2 ρv 2 a, where Va 2 = v R Θw v R Θw, and where w = w n, w e,. Therefore h diff x = 1 2 ρ v R Θw v R Θw. The associated Jacobian is given by C diff x = ρ GPS North sensor [R Θw] Θ The model for the GPS north sensor is 3 1 v R Θw v R Θw 3 1 I 2 2, 2 1 RΘ v R Θw. h GPS,n x = p n with associated Jacobian C GPS,n x = 1,,,,,,,,,,,,, GPS East sensor The model for the GPS east sensor is with associated Jacobian h GPS,e x = p e C GPS,e x =, 1,,,,,,,,,,,,. 15

16 3.3.5 GPS ground-speed sensor The inertial velocity in the world frame, projected onto the north-east plane is given by v g = [ I 2 2, 2 1 ] RΘv. 1 The ground-speed is given by V g v, Θ = v g. The model for the sensor is therefore where P = [ I 2 2, C GPS,Vg x = h GPS,Vg x = P RΘv, ] 2 1, and the associated Jacobian is given by 1 3, v R ΘP V P RΘ, gv,θ Θ, 1 3, 1 2, where Vgv,Θ Θ is computed numerically as V g v, Θ = V gv, Θ + e i V g v, Θ, Θ i where e i is the 3 1 vector with one in the i th location and zeros elsewhere, and is a small number GPS course The course angle is the direction of v g = v g n, v g e given in Equation 1. Therefore, the course angle is given by The model for the sensor is therefore χv, Θ = atan2v g e, v g n. h GPS,χ x = atan2v g e, v g n, and the associated Jacobian is given by C GPS,χ x = 1 3, where χv,v v χv,v v, χv,θ Θ, 1 3, 1 2, and χv,θ Θ are computed numerically as χv, Θ = χv + e i, Θ χv, Θ v i χv, Θ = χv, Θ + e i χv, Θ. Θ i 16

17 3.3.7 Pseudo Measurement for Side Slip Angle With the given sensors, the side-slip angle, or side-to-side velocity is not observable and can therefore drift. To help correct this situation, we can add a pseudo measurement on the side-slip angle by assuming that it is zero. The side slip angle is given by β = v r V a, therefore an equivalent condition is that v r =. The airspeed vector is given by v a = v RΘ w, which implies that v r v, Θ, w = [ 1 ] v RΘ w. Therefore, the model for the pseudo-sensor is given by h β x = v r v, Θ, w, and the associated Jacobian is given by C β x = 1 3, v rv,θ,w v, v rv,θ,w Θ, 1 3, where the partial derivatives are computed numerically as v r v, Θ, w = v rv + e i, Θ, w v r v, Θ, w v i v r v, Θ, w = v rv, Θ + e i, w v r v, Θ, w Θ i v r v, Θ, w = v rv, Θ, w + e i v r v, Θ, w. w i The measurement used in the correction step is y β =. 3.4 Algorithm for Direct Kalman Filter v rv,θ,w w,, We assume that the gyro, accelerometers, and pressure sensors are sampled at rate T s, which is the same rate that state estimates are required by the controller. We also assume that GPS is sampled at T GPS >> T s. 17

18 The algorithm for the direct Kalman filter is given as follows.. Initialize Filter. Set ˆx = p n, p e, p d, V a,,,,, ψ,,,,,, P = diag [e 2 p n, e 2 p e, e 2 p d, e 2 u, e 2 v, e 2 w, e 2 φ, e 2 θ, e 2 ψ, e 2 b x, e 2 b y, e 2 b z, e 2 w n, e 2 w e ], where e is the standard deviation of the expected error in. 1. At the fast sample rate T s 1.a. Propagate ˆx and P according to ˆx = fˆx, y P = Aˆx, yp + P A ˆx, y + G g ˆxQ gyros G g ˆx + G a Q accel G a + Q 1.b. Update ˆx and P with the static pressure sensor according to L = P C staticˆx /σ 2 abs pres + C static ˆx P C staticˆx P + = I LC static ˆx P ˆx + = ˆx + Ly abs pres h static ˆx. 1.c. Update ˆx and P with the differential pressure sensor according to L = P C diffˆx /σ 2 diff pres + C diff ˆx P C diffˆx P + = I LC diff ˆx P ˆx + = ˆx + Ly diff pres h diff ˆx. 1.d. Update ˆx and P with the side-slip pseudo measurement according to L = P C β ˆx /σ 2 β + C β ˆx P C β ˆx P + = I LC β ˆx P ˆx + = ˆx + L h β ˆx. 2. When GPS measurements are received at T GPS : 2.a. Update ˆx and P with the GPS north measurement according to L = P C GPS,nˆx /σ 2 GPS,n + C GPS,n ˆx P C GPS,nˆx P + = I LC GPS,n ˆx P ˆx + = ˆx + Ly GPS,n h GPS,n ˆx. 18

19 2.b. Update ˆx and P with the GPS east measurement according to L = P CGPS,eˆx /σgps,e 2 + C GPS,e ˆx P CGPS,eˆx P + = I LC GPS,e ˆx P ˆx + = ˆx + Ly GPS,e h GPS,e ˆx. 2.c. Update ˆx and P with the GPS groundspeed measurement according to L = P CGPS,V g ˆx /σgps,v 2 g + C GPS,Vg ˆx P CGPS,V g ˆx P + = I LC GPS,Vg ˆx P ˆx + = ˆx + Ly GPS,Vg h GPS,Vg ˆx. 2.d. Update ˆx and P with the GPS course measurement according to L = P CGPS,χˆx /σgps,χ 2 + C GPS,χ ˆx P CGPS,χˆx P + = I LC GPS,χ ˆx P ˆx + = ˆx + Ly GPS,χ h GPS,V χ ˆx. 19

20 4 Indirect Kalman Filter for Full States 4.1 Algorithm for the Indirect Kalman Filter The algorithm for the indirect Kalman filter is given as follows. Terms that differ from the direct Kalman filter are highlighted in blue.. Initialize Filter..1 Initialize the state estimate according to ˆx = p n, p e, p d, V a,,,,, ψ,,,,,..2 Initialize the error state estimate according to x=,,,,,,,,,,,,,, P = diag [e 2 p n, e 2 p e, e 2 p d, e 2 u, e 2 v, e 2 w, e 2 φ, e 2 θ, e 2 ψ, e 2 b x, e 2 b y, e 2 b z, e 2 w n, e 2 w e ], where e is the standard deviation of the expected error in. 1. At the fast sample rate T s 1.a. Propagate the estimated state ˆx according to 1.b. Propagate x and P according to ˆx = fˆx, y x= Aˆx, y x P = Aˆx, yp + P A ˆx, y + G g ˆxQ gyros G g ˆx + G a Q accel G a + Q 1.c. Update x and P with the static pressure sensor according to L = P C staticˆx /σ 2 abs pres + C static ˆx P C staticˆx P + = I LC static ˆx P x + = x + Ly abs pres h static ˆx C static ˆx x. 1.d. Update x and P with the differential pressure sensor according to L = P C diffˆx /σ 2 diff pres + C diff ˆx P C diffˆx P + = I LC diff ˆx P x + = x + Ly diff pres h diff ˆx C diff ˆx x. 2

21 1.e. Update ˆx and P with the side-slip pseudo measurement according to L = P C β ˆx /σ 2 β + C β ˆx P C β ˆx P + = I LC β ˆx P x + = x + L h β ˆx C β ˆx x. 2. When GPS measurements are received at T GPS : 2.a. Update ˆx and P with the GPS north measurement according to L = P C GPS,nˆx /σ 2 GPS,n + C GPS,n ˆx P C GPS,nˆx P + = I LC GPS,n ˆx P x + = x + Ly GPS,n h GPS,n ˆx C GPS,n ˆx x. 2.b. Update ˆx and P with the GPS east measurement according to L = P C GPS,eˆx /σ 2 GPS,e + C GPS,e ˆx P C GPS,eˆx P + = I LC GPS,e ˆx P x + = x + Ly GPS,e h GPS,e ˆx C GPS,e ˆx x. 2.c. Update ˆx and P with the GPS groundspeed measurement according to L = P C GPS,V g ˆx /σ 2 GPS,V g + C GPS,Vg ˆx P C GPS,V g ˆx P + = I LC GPS,Vg ˆx P x + = x + Ly GPS,Vg h GPS,Vg ˆx C GPS,Vg ˆx x. 2.d. Update ˆx and P with the GPS course measurement according to L = P C GPS,χˆx /σ 2 GPS,χ + C GPS,χ ˆx P C GPS,χˆx P + = I LC GPS,χ ˆx P x + = x + Ly GPS,χ h GPS,V χ ˆx C GPS,χ ˆx x. 2.e. Update ˆx according to ˆx + = ˆx + x +. 2.f. Set x =,,,,,,,,,,,,,. References 21