Page 203 - Innovations in Intelligent Machines

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 6.1) and position and course
(Section 6.2).

6.1 Roll and Pitch Estimation
From Eq. 3, the equations of motion for φ and θ are given by

˙
φ = p + q sin φ tan θ + r cos φ tan θ + ξ φ
˙
θ = q cos φ − r sin φ + ξ θ ,
where we have added the noise terms ξ φ ∼N(0,Q φ )and ξ θ ∼N(0,Q θ )to
model the sensor noise on p, q,and r. We will use the accelerometers as the
output equations. From Eq. (7), the output of the accelerometers is given by

⎛ ˙ u+gw−rv ⎞
g +sin θ
⎜ ˙ v+ru−pw ⎟
y accel = ⎜ − cos θ sin φ ⎟ + η accel . (27)
g
⎝ ⎠
˙ w+pv−qu − cos θ cos φ
g
However, since we do not have a method for directly measuring ˙u,˙v,˙w, u,
v,and w, we will assume that ˙u =˙v =˙w ≈ 0 and we will use Eq. (1) and
assume that α ≈ θ and β ≈ 0 to obtain
⎛ ⎞ ⎛ ⎞
u cos θ
⎝ v ⎠ ≈ V a ⎝ 0 ⎠ .
w sin θ
Substituting into Eq. (27) gives

qV a sin θ
⎛ ⎞
g +sin θ
⎜ rV a cos θ−pV a sin θ ⎟
y accel = ⎜ − cos θ sin φ ⎟ + η accel .
g
⎝ ⎠
−qV a cos θ
g − cos θ cos φ
T
T
T
T
Letting x =(φ, θ) , u =(p, q, r, V a ) , ξ =(ξ φ ,ξ θ ) ,and η =(η φ ,η θ ) ,weget
the nonlinear state equation
˙ x = f(x, u)+ ξ
y = h(x, u)+ η,
where
! "
p + q sin φ tan θ + r cos φ tan θ
f(x, u)=
q cos φ − r sin φ